Rough implementation of hough transform with sobel. Needs more work.

naive_hough.cpp

#include <fcntl.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <iostream>
#include <sstream>


#define DEG_QUANTIZE 3
#define HOUGH_THRESH 90 
#define THRESHOLD 25


#define DEBUG 1

#if DEBUG
  #define LOG(a) printf a
#else
  #define LOG(a) (void)0
#endif

#define PPM_LEX_DELIM '\n'
#define PPM_LEX_BEGIN_HEIGHT ' '
#define PPM_LEX_BEGIN_COMMENT '#'

enum PPM_LexerMode { PPM_TYPE, PPM_COMMENT, PPM_WIDTH, PPM_HEIGHT, PPM_COLORS };
enum PPM_Type { PPM_P5, PPM_P6 };

typedef double FLOAT;
typedef unsigned int UINT32;
typedef unsigned long long int UINT64;
typedef unsigned char UINT8;

typedef struct _PPM_Meta {
    char header[21];
    int header_length;
    PPM_Type type;
    int width;
    int height;    
    int colors;
} PPM_Meta;

static struct timeval t_diff, t_start, t_end, p_start, p_end, start, end;

// From: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
int timeval_diff(timeval *result, timeval x, timeval y)
{
    /* Perform the carry for the later subtraction by updating y. */
    if (x.tv_usec < y.tv_usec) {
        int nsec = (y.tv_usec - x.tv_usec) / 1000000 + 1;
        y.tv_usec -= 1000000 * nsec;
        y.tv_sec += nsec;
    }

    if (x.tv_usec - y.tv_usec > 1000000) {
        int nsec = (x.tv_usec - y.tv_usec) / 1000000;
        y.tv_usec += 1000000 * nsec;
        y.tv_sec -= nsec;
    }

    /* Compute the time remaining to wait. tv_usec is certainly positive. */
    result->tv_sec = x.tv_sec - y.tv_sec;
    result->tv_usec = x.tv_usec - y.tv_usec;

    /* Return 1 if result is negative. */
    return x.tv_sec < y.tv_sec;
}


void write_ppm(char filename_out[], PPM_Meta *ppm_meta, UINT8 **r_data, UINT8 **g_data, UINT8 **b_data) 
{
    FILE *fpout;
    int i;
    int j; 

    if ((fpout = fopen(filename_out, "w")) == NULL) {
        LOG(("Error writing %s\n", filename_out));
    }
    fwrite((void *)ppm_meta->header, ppm_meta->header_length, 1, fpout);

    for (i = 0; i < ppm_meta->height; ++i) {
        for (j = 0; j < ppm_meta->width; ++j) {
            fwrite((void *)&r_data[i][j], 1, 1, fpout);
            fwrite((void *)&g_data[i][j], 1, 1, fpout);
            fwrite((void *)&b_data[i][j], 1, 1, fpout);
        }
    }
    fclose(fpout);    
}

// Reads PNM file and returns header and image data as separate R, G, and B arrays.
// TODO(richard-to): Currently supports P6 format only.
void read_ppm(char filename_in[], PPM_Meta *ppm_meta, UINT8 **&r_data, UINT8 **&g_data, UINT8 **&b_data) 
{
    FILE *fpin; 

    char image_type[3];
    char image_width[5];
    char image_height[5];
    char image_colors[3];

    std::stringstream sstr;
    int sstr_val;

    int i = 0;
    int j = 0;
    int h = 0;

    char char_read;
    int result = 0;
    bool reading = true;
    
    PPM_LexerMode mode = PPM_TYPE;

    if ((fpin = fopen(filename_in, "r")) == NULL) {
        LOG(("Error opening %s\n", filename_in));
    }

    while (reading) {
        result = fread((void *)&char_read, 1, 1, fpin);

        if (result == 0) {
            LOG(("Error reading %s\n", filename_in));
            reading = false;
            break;
        } 

        if (mode != PPM_COMMENT && char_read != PPM_LEX_BEGIN_COMMENT) {
            ppm_meta->header[h++] = char_read;
        }

        if (mode == PPM_TYPE) {
            if (char_read == PPM_LEX_DELIM) {
                image_type[i] = '\0';
                mode = PPM_WIDTH;
                i = 0;
            } else {
                image_type[i] = char_read;
                ++i;
            }
        } else if (mode == PPM_WIDTH) {
            if (char_read == PPM_LEX_BEGIN_HEIGHT) {
                image_width[i] = '\0';
                mode = PPM_HEIGHT;
                i = 0;
            } else if (char_read == PPM_LEX_BEGIN_COMMENT) {
                mode = PPM_COMMENT;
                i = 0;
            } else {
                image_width[i] = char_read;
                ++i;
            }
        } else if (mode == PPM_HEIGHT) {
            if (char_read == PPM_LEX_DELIM) {
                image_height[i] = '\0';
                mode = PPM_COLORS;
                i = 0;
            } else {
                image_height[i] = char_read;
                ++i;
            }                             
        } else if (mode == PPM_COLORS) {
            if (char_read == PPM_LEX_DELIM) {
                image_colors[i] = '\0';                
                reading = false;
            } else {
                image_colors[i] = char_read;
                ++i;
            }
        } else if (mode == PPM_COMMENT) {
            if (char_read == PPM_LEX_DELIM) {
                mode = PPM_WIDTH;
                i = 0;
            }
        }
    }
    ppm_meta->header[h] = '\0';
    ppm_meta->header_length = h;

    ppm_meta->type = PPM_P6;

    sstr.str(image_width);
    sstr >> sstr_val;
    ppm_meta->width = sstr_val;

    sstr.clear();
    sstr.str(image_height);
    sstr >> sstr_val;
    ppm_meta->height = sstr_val;

    sstr.clear();
    sstr.str(image_colors);
    sstr >> sstr_val;
    ppm_meta->colors = sstr_val;

    r_data = new UINT8* [ppm_meta->height];
    g_data = new UINT8* [ppm_meta->height];
    b_data = new UINT8* [ppm_meta->height];

    for (int i = 0; i < ppm_meta->height; ++i) {
        r_data[i] = new UINT8[ppm_meta->width];
        g_data[i] = new UINT8[ppm_meta->width];
        b_data[i] = new UINT8[ppm_meta->width];
    }

    for (i = 0; i < ppm_meta->height; ++i) {
        for (j = 0; j < ppm_meta->width; ++j) {
            fread((void *)&r_data[i][j], 1, 1, fpin);
            fread((void *)&g_data[i][j], 1, 1, fpin);
            fread((void *)&b_data[i][j], 1, 1, fpin);
        }
    }
    fclose(fpin);
}

// Generic PSF function
// TODO(richard-to): Not sure if this is working properly
void psf(FLOAT kernel[], PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&data_out)
{   
    int i;
    int j;
    int h = ppm_meta->height - 1;
    int w = ppm_meta->width - 1;    
    
    FLOAT value;
    for (i = 1; i < h; ++i) {
        for (j = 1; j < w; ++j) {
            value  = (kernel[0] * (FLOAT)data_in[i - 1][j - 1]);
            value += (kernel[1] * (FLOAT)data_in[i - 1][j]);
            value += (kernel[2] * (FLOAT)data_in[i - 1][j + 1]);

            value += (kernel[3] * (FLOAT)data_in[i][j - 1]);
            value += (kernel[4] * (FLOAT)data_in[i][j]);
            value += (kernel[5] * (FLOAT)data_in[i][j + 1]);

            value += (kernel[6] * (FLOAT)data_in[i + 1][j - 1]);
            value += (kernel[7] * (FLOAT)data_in[i + 1][j]);
            value += (kernel[8] * (FLOAT)data_in[i + 1][j + 1]);

            if (value < 0) {
                value = 0;
            }

            if (value > ppm_meta->colors) {
                value = ppm_meta->colors;
            }

            data_out[i][j] = (UINT8)value;            
        }
    }    
}

void hough(int deg_incr, int thresh, PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&data_out)
{
    int i;
    int j;
    int k;
    int p;

    int h = ppm_meta->height;
    int w = ppm_meta->width;  

    FLOAT deg_pi = 180.0;
    int acc_mid = ppm_meta->width * ppm_meta->height;
    int acc_size = 2 * acc_mid;
    int **accumulator = new int*[acc_size];
    int points_size = deg_pi/(int)deg_incr;
    FLOAT *points = new FLOAT[points_size];

    for (i = 0; i < points_size; ++i) {
        points[i] = M_PI * (i * deg_incr / deg_pi);
    }

    for (i = 0; i < acc_size; ++i) {
        accumulator[i] = new int[points_size];
        for (j = 0; j < points_size; ++j) {
            accumulator[i][j] = 0;
        }
    }

    for (i = 0; i < h; ++i) {
        for (j = 0; j < w; ++j) {
            if ((int)data_in[i][j] > 0) {
                for (k = 0; k < points_size; ++k) {
                    p = round(i * sin(points[k]) + j * cos(points[k]));
                    accumulator[p + acc_mid][k]++;
                }
            }            
        }
    }

    for (i = 0; i < h; ++i) {
        for (j = 0; j < w; ++j) {
            if ((int)data_in[i][j] > 0) {
                for (k = 0; k < points_size; ++k) {
                    p = round(i * sin(points[k]) + j * cos(points[k]));
                    if (accumulator[p + acc_mid][k] > thresh) {
                        data_out[i][j] = ppm_meta->colors;
                    }
                }
            }            
        }
    }    
}

void draw_hough_lines(PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&r_data, UINT8 **&g_data, UINT8 **&b_data) 
{
    int i;
    int j;
    int k;
    int p;

    int h = ppm_meta->height;
    int w = ppm_meta->width; 

    for (i = 0; i < h; ++i) {
        for (j = 0; j < w; ++j) {
            if ((int)data_in[i][j]  > 0) {
                r_data[i][j] = ppm_meta->colors;
                g_data[i][j] = 0;
                b_data[i][j] = 0;
            }            
        }
    } 
}

void sobel(PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&data_out) 
{   
    int i;
    int j;
    int h = ppm_meta->height - 1;
    int w = ppm_meta->width - 1;    
    
    FLOAT grad_x;
    FLOAT grad_y;
    
    for (i = 1; i < h; ++i) {
        for (j = 1; j < w; ++j) {
            grad_x  = 
                (
                    (FLOAT)data_in[i - 1][j - 1] +
                    2.0 * (FLOAT)data_in[i - 1][j] +
                    (FLOAT)data_in[i - 1][j + 1]
                ) -
                (
                    (FLOAT)data_in[i + 1][j - 1] +
                    2.0 * (FLOAT)data_in[i + 1][j] +
                    (FLOAT)data_in[i + 1][j + 1]
                );

            grad_y  = 
                (
                    (FLOAT)data_in[i - 1][j - 1] +
                    2.0 * (FLOAT)data_in[i][j - 1] +
                    (FLOAT)data_in[i + 1][j - 1]
                ) -
                (
                    (FLOAT)data_in[i - 1][j + 1] +
                    2.0 * (FLOAT)data_in[i][j + 1] +
                    (FLOAT)data_in[i + 1][j + 1]
                );

            data_out[i][j] = (UINT8)(sqrt(grad_x * grad_x + grad_y * grad_y) / 5.66);
        }
    }
}

void threshold(PPM_Meta *ppm_meta, int thresh, UINT8 **data_in, UINT8 **&data_out) 
{   
    int i;
    int j;
    int h = ppm_meta->height;
    int w = ppm_meta->width;    
    
    for (i = 0; i < h; ++i) {
        for (j = 0; j < w; ++j) {   
            if ((int)data_in[i][j] > thresh) {
                data_out[i][j] = ppm_meta->colors;
            } else {
                data_out[i][j] = 0;
            }
        }
    }
}

void grayscale(PPM_Meta *ppm_meta, UINT8 **r_data, UINT8 **g_data, UINT8 **b_data,  UINT8 **&data_out) 
{
    FLOAT r_weight = 0.21;
    FLOAT g_weight = 0.71;
    FLOAT b_weight = 0.07;
    FLOAT value;

    int i;
    int j;
    int h = ppm_meta->height;
    int w = ppm_meta->width;    
    
    for (i = 0; i < h; ++i) {
        for (j = 0; j < w; ++j) {  
            value = r_data[i][j] * r_weight + g_data[i][j] * g_weight; + b_data[i][j] * b_weight;
            data_out[i][j] = (UINT8)value;
        }
    }    
}


void gaussian_blur(PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&data_out) 
{   
    FLOAT kernel[] = {
        0.0625, 0.125, 0.0625,
        0.125, 0.25, 0.125,
        0.0625, 0.125, 0.0625,
    };    
    psf(kernel, ppm_meta, data_in, data_out);    
}

void create_pixel_array(PPM_Meta *ppm_meta, UINT8 **&data_out)
{
    int i;

    data_out = new UINT8* [ppm_meta->height];
    for (i = 0; i < ppm_meta->height; ++i) {
        data_out[i] = new UINT8[ppm_meta->width];
    }
}

void copy_pixel_array(PPM_Meta *ppm_meta, UINT8 **data_in, UINT8 **&data_out)
{
    int i;
    int j;

    data_out = new UINT8* [ppm_meta->height];
    for (i = 0; i < ppm_meta->height; ++i) {
        data_out[i] = new UINT8[ppm_meta->width];
        for (j = 0; j < ppm_meta->width; ++j) {
            data_out[i][j] = data_in[i][j];
        }
    }  
}

void delete_pixel_array(PPM_Meta *ppm_meta, UINT8 **&data)
{
    int i;
    for (i = 0; i < ppm_meta->height; ++i) {
        delete data[i];
    }
    delete data;
}

int main(int argc, char *argv[])
{
    PPM_Meta ppm_meta;
    UINT8 **r_data;
    UINT8 **g_data;
    UINT8 **b_data;

    UINT8 **r_out;
    UINT8 **g_out;
    UINT8 **b_out;

    UINT8 **grayscale_data;
    UINT8 **blur_data;
    UINT8 **sobel_data;
    UINT8 **threshold_data;

    UINT8 **hough_data;

    if (argc < 3) {
        printf("Usage: naive_hough input_file.ppm output_file.ppm\n");
        exit(-1);        
    }

    read_ppm(argv[1], &ppm_meta, r_data, g_data, b_data); 

    create_pixel_array(&ppm_meta, grayscale_data);
    grayscale(&ppm_meta, r_data, g_data, b_data, grayscale_data);

    copy_pixel_array(&ppm_meta, grayscale_data, blur_data);
    gaussian_blur(&ppm_meta, grayscale_data, blur_data);   
    
    copy_pixel_array(&ppm_meta, blur_data, sobel_data);    
    sobel(&ppm_meta, blur_data, sobel_data);

    copy_pixel_array(&ppm_meta, sobel_data, threshold_data);    
    threshold(&ppm_meta, THRESHOLD, sobel_data, threshold_data);

    create_pixel_array(&ppm_meta, hough_data);    
    hough(DEG_QUANTIZE, HOUGH_THRESH, &ppm_meta, threshold_data, hough_data);
    draw_hough_lines(&ppm_meta, hough_data, r_data, g_data, b_data);

    write_ppm(argv[2], &ppm_meta, r_data, g_data, b_data);

    delete_pixel_array(&ppm_meta, r_data);
    delete_pixel_array(&ppm_meta, g_data);
    delete_pixel_array(&ppm_meta, b_data);
    
    delete_pixel_array(&ppm_meta, blur_data);
    delete_pixel_array(&ppm_meta, sobel_data);
    delete_pixel_array(&ppm_meta, threshold_data);
    delete_pixel_array(&ppm_meta, hough_data);
}