static int run_cascade_classifier(cascade_t* cascade, point_t pt)
{
int win_w = cascade->window.w;
int win_h = cascade->window.h;
uint32_t n = (win_w * win_h);
uint32_t i_s = imlib_integral_mw_lookup (cascade->sum, pt.x, 0, win_w, win_h);
uint32_t i_sq = imlib_integral_mw_lookup(cascade->ssq, pt.x, 0, win_w, win_h);
uint32_t m = i_s/n;
uint32_t v = i_sq/n-(m*m);
// Skip homogeneous regions.
if (v<(50*50)) {
return 0;
}
cascade->std = fast_sqrtf(i_sq*n-(i_s*i_s));
for (int i=0, w_idx=0, r_idx=0, t_idx=0; i<cascade->n_stages; i++) {
int stage_sum = 0;
for (int j=0; j<cascade->stages_array[i]; j++, t_idx++) {
// Send the shifted window to a haar filter
stage_sum += eval_weak_classifier(cascade, pt, t_idx, w_idx, r_idx);
w_idx += cascade->num_rectangles_array[t_idx];
r_idx += cascade->num_rectangles_array[t_idx] * 4;
}
// If the sum is below the stage threshold, no objects were detected
if (stage_sum < (cascade->threshold * cascade->stages_thresh_array[i])) {
return 0;
}
}
return 1;
}
static float calculate_TA(void)
{
uint16_t ptat=0;
uint8_t cmd_buf[4]={MLX_READ_REG, 0x90, 0x00, 0x01};
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)&ptat, 2, true);
return (-k_t1 + fast_sqrtf(k_t1_sq - (4 * k_t2 * (v_th - ptat)))) / (2 * k_t2) + 25;
}
static void mlx90620_read_to(float *t)
{
float v_ir_comp;
float v_ir_off_comp;
float v_ir_tgc_comp;
int16_t cpix;
uint8_t cmd_buf[4];
int16_t ir_data[64];
// static int count=0;
// if (count++ %16 ==0) {
float Ta = calculate_TA();
// (T+273.15f)^4
float Ta4 = (Ta + 273.15f) * (Ta + 273.15f) * (Ta + 273.15f) * (Ta + 273.15f);
// }
// Read IR data
memcpy(cmd_buf, (uint8_t [4]){MLX_READ_REG, 0x00, 0x01, 0x40}, sizeof(cmd_buf)); //read 64*2 bytes
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)ir_data, 128, true);
// Read compensation data
memcpy(cmd_buf, (uint8_t [4]){MLX_READ_REG, 0x91, 0x00, 0x01}, sizeof(cmd_buf));
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)&cpix, 2, true);
//Calculate the offset compensation for the one compensation pixel
//This is a constant in the TO calculation, so calculate it here.
float v_cp_off_comp = (float)cpix - (a_cp + (b_cp/(2<<(b_i_scale-1))) * (Ta - 25));
for (int i=0; i<64; i++) {
//#1: Calculate Offset Compensation
v_ir_off_comp = ir_data[i] - (a_ij[i] + (float)(b_ij[i]/(2<<(b_i_scale-1))) * (Ta - 25));
//#2: Calculate Thermal Gradien Compensation (TGC)
v_ir_tgc_comp = v_ir_off_comp - ( ((float)tgc/32) * v_cp_off_comp);
//#3: Calculate Emissivity Compensation
v_ir_comp = v_ir_tgc_comp / emissivity;
t[i] = fast_sqrtf(fast_sqrtf(v_ir_comp/alpha_ij[i] + Ta4)) - 273.15f;
}
}
inline float length(float center_x, float center_y, float target_x, float target_y)
{
return ( fast_sqrtf( length2(center_x, center_y, target_x, target_y) ) );
}
inline float length(const sf::Vector2f& pos)
{
return ( fast_sqrtf( length2(pos) ) );
}
inline float length(float x, float y)
{
return ( fast_sqrtf( length2(x, y) ) );
}
void imlib_edge_canny(image_t *src, rectangle_t *roi, int low_thresh, int high_thresh)
{
int w = src->w;
gvec_t *gm = fb_alloc0(roi->w*roi->h*sizeof*gm);
//1. Noise Reduction with a Gaussian filter
imlib_sepconv3(src, kernel_gauss_3, 1.0f/16.0f, 0.0f);
//2. Finding Image Gradients
for (int gy=1, y=roi->y+1; y<roi->y+roi->h-1; y++, gy++) {
for (int gx=1, x=roi->x+1; x<roi->x+roi->w-1; x++, gx++) {
int vx=0, vy=0;
// sobel kernel in the horizontal direction
vx = src->data [(y-1)*w+x-1]
- src->data [(y-1)*w+x+1]
+ (src->data[(y+0)*w+x-1]<<1)
- (src->data[(y+0)*w+x+1]<<1)
+ src->data [(y+1)*w+x-1]
- src->data [(y+1)*w+x+1];
// sobel kernel in the vertical direction
vy = src->data [(y-1)*w+x-1]
+ (src->data[(y-1)*w+x+0]<<1)
+ src->data [(y-1)*w+x+1]
- src->data [(y+1)*w+x-1]
- (src->data[(y+1)*w+x+0]<<1)
- src->data [(y+1)*w+x+1];
// Find magnitude
int g = (int) fast_sqrtf(vx*vx + vy*vy);
// Find the direction and round angle to 0, 45, 90 or 135
int t = (int) fast_fabsf((atan2f(vy, vx)*180.0f/M_PI));
if (t < 22) {
t = 0;
} else if (t < 67) {
t = 45;
} else if (t < 112) {
t = 90;
} else if (t < 160) {
t = 135;
} else if (t <= 180) {
t = 0;
}
gm[gy*roi->w+gx].t = t;
gm[gy*roi->w+gx].g = g;
}
}
// 3. Hysteresis Thresholding
// 4. Non-maximum Suppression and output
for (int gy=0, y=roi->y; y<roi->y+roi->h; y++, gy++) {
for (int gx=0, x=roi->x; x<roi->x+roi->w; x++, gx++) {
int i = y*w+x;
gvec_t *va=NULL, *vb=NULL, *vc = &gm[gy*roi->w+gx];
// Clear the borders
if (y == (roi->y) || y == (roi->y+roi->h-1) ||
x == (roi->x) || x == (roi->x+roi->w-1)) {
src->data[i] = 0;
continue;
}
if (vc->g < low_thresh) {
// Not an edge
src->data[i] = 0;
continue;
// Check if strong or weak edge
} else if (vc->g >= high_thresh ||
gm[(gy-1)*roi->w+(gx-1)].g >= high_thresh ||
gm[(gy-1)*roi->w+(gx+0)].g >= high_thresh ||
gm[(gy-1)*roi->w+(gx+1)].g >= high_thresh ||
gm[(gy+0)*roi->w+(gx-1)].g >= high_thresh ||
gm[(gy+0)*roi->w+(gx+1)].g >= high_thresh ||
gm[(gy+1)*roi->w+(gx-1)].g >= high_thresh ||
gm[(gy+1)*roi->w+(gx+0)].g >= high_thresh ||
gm[(gy+1)*roi->w+(gx+1)].g >= high_thresh) {
vc->g = vc->g;
} else { // Not an edge
src->data[i] = 0;
continue;
}
switch (vc->t) {
case 0: {
va = &gm[(gy+0)*roi->w+(gx-1)];
vb = &gm[(gy+0)*roi->w+(gx+1)];
break;
}
case 45: {
va = &gm[(gy+1)*roi->w+(gx-1)];
vb = &gm[(gy-1)*roi->w+(gx+1)];
break;
}
case 90: {
va = &gm[(gy+1)*roi->w+(gx+0)];
vb = &gm[(gy-1)*roi->w+(gx+0)];
break;
}
case 135: {
va = &gm[(gy+1)*roi->w+(gx+1)];
vb = &gm[(gy-1)*roi->w+(gx-1)];
break;
}
}
if (!(vc->g > va->g && vc->g > vb->g)) {
src->data[i] = 0;
} else {
src->data[i] = 255;
}
}
}
fb_free();
}
