i_gaussian.cpp 6.19 KB
//
//  Created by Arnaud Blanchard on 22/12/15.
//  Copyright ETIS 2015. All rights reserved.
//
#include "blc_channel.h"
#include "blc_program.h"
#include <unistd.h>
#include <math.h>

#define DEFAULT_OUTPUT_NAME ":<derivative>gaussian<pid>"

// gain/(2PI.sigma^2)*exp(-(x^2+y^2)/(2sigma^2))
void gaussian(float *output, int width, int  height, float sigma, float gain){
    int i, j;
    float x, y;
    
    FOR(j, height){
        FOR(i, width){
            x=i-width/2.0+0.5;
            y=j-height/2.0+0.5;
            
            output[i+j*width]=gain/(M_2_PI*sigma*sigma)*exp(-(x*x+y*y)/(2*sigma*sigma));
        }
    }
}

void dxgaussian(float *output, int width, int  height, float sigma, float gain){
    int i, j;
    float x,y;
    
    FOR(j, height){
        FOR(i, width){
            x=i-width/2.0+0.5;
            y=j-height/2.0+0.5;
            output[i+j*width]=-gain*x/(M_2_PI*pow(sigma, 4))*exp(-(x*x+y*y)/(2*sigma*sigma));
        }
    }
}

//-gain*y/(2*sigma^4*PI)*exp(-(x^2+y^2)/(2*sigma^2))
void dygaussian(float *output, int width, int  height, float sigma, float gain){
    int i, j;
    float x, y;
    
    FOR(j, height){
        FOR(i, width){
            x=i-width/2.0+0.5;
            y=j-height/2.0+0.5;
            output[i+j*width]=-gain*y/(M_2_PI*pow(sigma, 4))*exp(-(x*x+y*y)/(2*sigma*sigma));
        }
    }
}

//	gain/(pi*sigma²)(1-1/2(x²+y²)/sigma²)*exp(-1/2*(x²+y²)/sigma²)
void d2gaussian(float *output, int width, int  height, float sigma, float gain){
    int i, j;
    float x, y, coeff;
    
    FOR(j, height){
        FOR(i, width){
            x=i-(float)width/2.0+0.5;
            y=j-(float)height/2.0+0.5;
            coeff=(x*x+y*y)/(float)(2*sigma*sigma); //(x²+y²)/(2*sigma²)
            output[i+j*width]=gain/(M_PI*sigma*sigma)*(1-coeff)*exp(-coeff);
        }
    }
}


int main(int argc, char **argv){
    blc_channel output;
    char const  *output_name, *size_str, *type_str, *sigma_str, *gain_str, *derivative_str, *xderivative_str, *yderivative_str, *period_str;
    char const *prefix;
    float sigma,  gain;
    int executed, period;
    uint32_t type;
    
    blc_program_set_description("Create second derivative gaussian");
    blc_program_add_option(&gain_str, 'g', "gain", "real", "gain to apply to the filter", "1");
    blc_program_add_option(&output_name, 'o', "output", "blc_channel-out", "channel name", DEFAULT_OUTPUT_NAME);
    blc_program_add_option(&period_str, 'p', "period", "integer", "refresh period in ms (-1 for blocking, 0 for quitting immediatly)", "-1");
    blc_program_add_option(&size_str, 's', "size", "(integer)[x(integer)]", "size of the output 1 or 2D", "3x3");
    blc_program_add_option(&type_str, 't', "type", "FL32", "type of values", "FL32");
    blc_program_add_option(&derivative_str, 'D', "derivative", "integer", "all axes derivative", NULL);
    blc_program_add_option(&xderivative_str, 'x', "xderivative", "integer", "x axis derivative", NULL);
    blc_program_add_option(&yderivative_str, 'y', "yderivative", "integer", "y axis derivative", NULL);
    blc_program_add_option(&sigma_str, 'S', "sigma", "real", "sigma of the initial gaussian (default -1 is for min(size)/6", "-1");
    /*  blc_program_add_option(&xderivative_str, 'X', "xsigma", "real", "sigma for x axis", NULL);
     blc_program_add_option(&yderivative_str, 'Y', "ysigma", "real", "sigma for y axis", NULL);*/
    blc_program_init(&argc, &argv, blc_quit);
    blc_command_forward_blc_channels();

    if (derivative_str) prefix="d2";
    else if(xderivative_str) prefix="dx";
    else if(yderivative_str) prefix="dy";
    else prefix="";
    
    if (strcmp(output_name, DEFAULT_OUTPUT_NAME)==0) SYSTEM_ERROR_CHECK(asprintf((char**)&output_name, ":%sgaussian%d", prefix, getpid()),-1, NULL);
    
    type=STRING_TO_UINT32(type_str);
    
    SSCANF(1, gain_str, "%f", &gain);
    SSCANF(1, period_str, "%d", &period);
    if (period==-1) period=-2; //We past at least one step before blocking on keyboard
    else if (period!=0) EXIT_ON_ERROR("period can only be 0 for non blocking and -1 for blocking. However it is '%d'", period);
    
    output.create_or_open(output_name, BLC_CHANNEL_WRITE, type, 'NDEF', size_str);
    if (strcmp(sigma_str, "-1")==0) sigma=MIN(output.dims[0].length, output.dims[1].length)/6.;
    else SYSTEM_SUCCESS_CHECK(sscanf(sigma_str, "%f", &sigma), 1, "Error parsing '%s'", sigma_str);
    
    if (output.sem_ack_data) output.publish(); //If no need for data acknowledgement, we publish the blc_channel once it has been updated.
    //Synchronize the BLC_COMMAND_LOOP with the output channel
    blc_loop_try_add_waiting_semaphore(output.sem_ack_data);
    blc_loop_try_add_posting_semaphore(output.sem_new_data);
    
    //We use the BLC_COMMAND_LOOP as it manages locked semaphores eventhough when we loop only once.
    BLC_COMMAND_LOOP(period){
        executed=0;
        if (xderivative_str==NULL && yderivative_str ==NULL && derivative_str==NULL) {
            gaussian(output.floats, output.dims[0].length, output.dims[1].length, sigma, gain);
        }
        if (derivative_str){
            if (strcmp(derivative_str, "2")==0) d2gaussian(output.floats, output.dims[0].length, output.dims[1].length, sigma, gain);
            else EXIT_ON_ERROR("derivative '%s' not possible only '2' is", derivative_str);
            executed++;
        }
        if (xderivative_str) {
            if(strcmp(xderivative_str, "1")==0) dxgaussian(output.floats, output.dims[0].length, output.dims[1].length, sigma, gain);
            else EXIT_ON_ERROR("xderivative '%s' not possible only '1' is", xderivative_str);
            executed++;
        }
        if (yderivative_str){
            if (strcmp(yderivative_str, "1")==0) dygaussian(output.floats, output.dims[0].length, output.dims[1].length, sigma, gain);
            else EXIT_ON_ERROR("yderivative '%s' not possible only '1' is", yderivative_str);
            executed++;
            
        }
        if (executed>1) EXIT_ON_ERROR("You cannot have more than one derivative option");
        
        //If we haven't yet published the blc_channel we do it now that it is updated
        if (output.sem_ack_data==NULL && blc_loop_iteration==0) output.publish();
        
        if(period==0) blc_command_ask_quit();
    };
    

    return EXIT_SUCCESS;
}