X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fsensorboard%2Fimg_processing.c;fp=projects%2Fmicrob2010%2Fsensorboard%2Fimg_processing.c;h=0000000000000000000000000000000000000000;hp=d030478561019dfca9cfa54fa6fb053d01e81bb6;hb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305;hpb=821f753c0f88aff895d9feae59c442a6c446f96b diff --git a/projects/microb2010/sensorboard/img_processing.c b/projects/microb2010/sensorboard/img_processing.c deleted file mode 100644 index d030478..0000000 --- a/projects/microb2010/sensorboard/img_processing.c +++ /dev/null @@ -1,1619 +0,0 @@ -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include - -#include "img_processing.h" - - -#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) - -#ifndef HOST_VERSION - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "main.h" - -#define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args) -#define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args) -#define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args) - -#else - -#define IMG_DEBUG(args...) debug_printf(args) -#define IMG_NOTICE(args...) debug_printf(args) -#define IMG_ERROR(args...) debug_printf(args) - -#endif - - - - -#define OBJECT_MINIMUM_DEMI_PERIMETER (2*3) -/* store object in pool if never seen - * returns: - * 1 if new object - * 0 if already known - */ -int store_obj(Object_bb* tab_o, int total, Object_bb*o) -{ - uint8_t i; - - if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER) - return 0; - - for (i=0;ix_max = 0; - o->y_max = 0; - o->x_min = x_in; - o->y_min = y_in; - - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - if (pos_xx_min) - o->x_min = pos_x; - if (pos_yy_min) - o->y_min = pos_y; - if (pos_x>o->x_max) - o->x_max = pos_x; - if (pos_y>o->y_max) - o->y_max = pos_y; - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->len = len; -} - - - - - -/* step around object of given color and computes its polygon */ -void object_find_poly(unsigned char* data, - int16_t x_in, int16_t y_in, - int16_t start_x, int16_t start_y, - int16_t color, int16_t color_w, Object_poly* o) -{ - int16_t pos_x = start_x; - int16_t pos_y = start_y; - int16_t start =0; - int16_t len = 0; - int16_t count_stand = 0; - uint16_t pt_step, pt_num; - vect_t v, vi; - - v.x = 1; - v.y = 0; - - vi = v; - - pt_step = o->len/POLY_MAX_PTS + 1; - - pt_num = 0; - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - - if (len >pt_num*pt_step){ - o->pts[pt_num].x = pos_x; - o->pts[pt_num].y = pos_y; - pt_num+=1; - if (pt_num>=POLY_MAX_PTS) - break; - } - - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->pts_num = pt_num; - -} - -#define PT_LEFT 0 -#define PT_RIGHT 2 -#define PT_TOP 1 -#define PT_DOWN 3 - -/* return most left/right/top/down pts indexes of given polygon */ -void object_find_extrem_points_index(Object_poly* o, - unsigned int *pts) -{ - unsigned int i; - for (i=0;i<4;i++) - pts[i] = 0; - - - for (i=1;ipts_num;i++){ - if (o->pts[i].x < o->pts[pts[PT_LEFT]].x) - pts[PT_LEFT] = i; - if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x) - pts[PT_RIGHT] = i; - if (o->pts[i].y < o->pts[pts[PT_TOP]].y) - pts[PT_TOP] = i; - if (o->pts[i].y > o->pts[pts[PT_DOWN]].y) - pts[PT_DOWN] = i; - } - -} - - -#define NUM_CALIPERS 4 -/* for debug purpose: display a vector on image */ -void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, - vect_t *v, point_t p) -{ - unsigned int i; - float n; - int16_t x, y; - float coef=1.0; - - if (!v->x && !v->y) - return; - - n = vect_norm(v); - coef = 1/n; - for (i=0;i<5;i++){ - x = p.x; - y = p.y; - - x+=(float)((v->x)*(float)i*(float)coef); - y+=(float)((v->y)*(float)i*(float)coef); - if ((x== p.x) && (y == p.y)) - buf[y*x_in+x] = 0x0; - else - buf[y*x_in+x] = 0x0; - } - -} - -#define CAL_X 3 -#define CAL_Y 0 - - -/* compute minimum rectangle area including the given convex polygon */ -void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, - vect_t *v_out, vect_t *r1, vect_t*r2) -{ - vect_t calipers[NUM_CALIPERS]; - vect_t edges[NUM_CALIPERS]; - - unsigned int i; - unsigned int calipers_pts_index[NUM_CALIPERS]; - float angles[NUM_CALIPERS]; - float min_angle; - float total_rot_angle = 0; - int caliper_result_index; - - vect_t res1, res2; - float ps, n1, n2, caliper_n; - - float aera_tmp; - /* XXX hack sould be max*/ - float aera_min=0x100000; - - object_find_extrem_points_index(o, calipers_pts_index); - - calipers[0].x = 0; - calipers[0].y = 1; - - calipers[1].x = -1; - calipers[1].y = 0; - - calipers[2].x = 0; - calipers[2].y = -1; - - calipers[3].x = 1; - calipers[3].y = 0; - - - while (total_rot_angle <= M_PI/2){ - - for (i=0;ipts[(calipers_pts_index[i] + 1)%o->pts_num].x - - o->pts[calipers_pts_index[i]].x; - edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y - - o->pts[calipers_pts_index[i]].y; - - /* compute angle between caliper and polygon edge */ - angles[i] = vect_get_angle(&edges[i], &calipers[i]); - } - - /* find min angle */ - min_angle = angles[0]; - caliper_result_index = 0; - for (i=1;ipts_num; i++){ - calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS]; - vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]); - } - - /* update calipers point */ - for (i=0;ipts_num; - } - - res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x; - res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y; - - res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x; - res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y; - - ps = vect_pscal(&res1, &calipers[CAL_X]); - n1 = vect_norm(&res1); - caliper_n = vect_norm(&calipers[CAL_X]); - caliper_n*=caliper_n; - - res1 = calipers[CAL_X]; - - res1.x *= ps/(caliper_n); - res1.y *= ps/(caliper_n); - - - ps = vect_pscal(&res2, &calipers[CAL_Y]); - n1 = vect_norm(&res2); - - res2 = calipers[CAL_Y]; - - res2.x *= ps/(caliper_n); - res2.y *= ps/(caliper_n); - - n1 = vect_norm(&res1); - n2 = vect_norm(&res2); - - aera_tmp = n1*n2; - - if (aera_min >aera_tmp){ - aera_min = aera_tmp; - for (i=0;ipts[pts_index_out[i]].x; - p1.y = o->pts[pts_index_out[i]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l1); - - vect_rot_trigo(&caliper_tmp); - - p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x; - p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l2); - - ret = intersect_line(&l1, &l2, &p_int1); - if (ret!=1) - return 0; - //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y); - - mp_x+=p_int1.x; - mp_y+=p_int1.y; - - } - - - - p->x = lround(mp_x/NUM_CALIPERS); - p->y = lround(mp_y/NUM_CALIPERS); - - - return 1; - -} - -#define OBJECT_DIM 5 -/* split zone in many column's sized area */ -int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color) -{ - int n1, n2; - int i, j; - int index=0; - int r1_s, r2_s; - point_t ptmp; - - - n1 = vect_norm(r1); - n2 = vect_norm(r2); - - r1_s = n1/OBJECT_DIM; - r2_s = n2/OBJECT_DIM; - - if (!r1_s || ! r2_s) - return 0; - - ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2)); - ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2)); - - for(i=0;ix)/r1_s+(j*r2->x)/r2_s; - zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s; - zones[index].h = color; - zones[index].valid = 1; - - index++; - if (index>=max_zone) - return index; - - - } - } - - return index; -} - -#define OBJECT_SEMI_DIM (OBJECT_DIM/2) -#define MIN_SURFACE_PERCENT 50 -#define HIGHER_MAX_PIXEL 5 - - -int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z) -{ - int x, y; - uint16_t count, total_pix, higher_pixels; - - count = 0; - total_pix=0; - higher_pixels = 0; - - for (x = -OBJECT_SEMI_DIM; - (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - x++){ - for (y = -OBJECT_SEMI_DIM; - (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - y++){ - total_pix++; - if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h) - count++; - - if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h) - higher_pixels++; - - } - - } - - IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y, - count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels); - - if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) && - (higher_pixels (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin)) - continue; - - p[i].valid = 0; - - } - - return zones_num; -} - -#define MAX_DIST_TO_ZONE 2 - -unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone) -{ - int i; - - for (i = 0; i < zones_num; i++){ - - IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x); - - if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone)) - continue; - - p[i].valid = 0; - } - - return zones_num; -} - - -/* delete point to render polygon convex */ -int object_poly_to_convex(Object_poly *o) -{ - unsigned int i, j; - vect_t v, w; - int16_t z; - unsigned int del_pts_num = 0; - - for (i=0;ipts_num;){ - v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x; - v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y; - - w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x; - w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y; - - z = vect_pvect(&v, &w); - if (z>0){ - i+=1; - continue; - } - - /* found a convex angle (or colinear points) */ - for (j = i; j < o->pts_num-1; j++){ - o->pts[j] = o->pts[j+1]; - } - if (i!=0) - i-=1; - o->pts_num--; - del_pts_num++; - } - - return del_pts_num; -} - - - - - -#define DEFAULT_COLOR 255 -/* scan all image and find objects*/ -unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, - Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj) -{ - int16_t i, obj_num; - - uint8_t in_color=0; - int16_t etat; - - Object_bb o; - int ret; - - obj_num = 0; - /* - first, delete borders - */ - for (i=0;i= MAX_SISTER_PER_ZONE) - break; - - - if (!zones[i].valid) - continue; - - - if (i == z_n) - continue; - - /* twin tower must have same high */ - if (zones[i].h != zones[z_n].h) - continue; - - IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) + - (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) ); - - - IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER); - - /* - twin tower must be close/far enought to drop lintel - */ - if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE) - continue; - - - pts2line(&zones[i].p, &zones[z_n].p, &l); - - /* - test the paralelism of the temple: - zone may be on same distance from center - */ - - - v1.x = zones[z_n].p.x - center_x; - v1.y = zones[z_n].p.y - center_y; - - dist = vect_norm(&v1); - - v2.x = zones[i].p.x - center_x; - v2.y = zones[i].p.y - center_y; - - dist2 = vect_norm(&v2); - - IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2); - if (ABS(dist-dist2) > 3){ - IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2)); - continue; - } - - - - - /* no other aligned tower to avoid dropping on a lintel - * (3 aligned zone may mean lintel) - */ - - good_zone = 1; - - for (j = 0; j < zones_num; j++){ - if (j==i ||j == z_n) - continue; - - /* if third zone, but lower */ - if (zones[j].h <= zones[i].h) - continue; - - /* - check distance from dropping zone to - line formed by twin towers - */ - - proj_pt_line(&zones[j].p, &l, &p); - - - /* test if projected point is in the segement */ - - - v.x = zones[z_n].p.x - zones[i].p.x; - v.y = zones[z_n].p.y - zones[i].p.y; - - v1.x = p.x - zones[i].p.x; - v1.y = p.y - zones[i].p.y; - - n1 = vect_pscal_sign(&v, &v1); - - v.x = -v.x; - v.y = -v.y; - - v1.x = p.x - zones[z_n].p.x; - v1.y = p.y - zones[z_n].p.y; - - - n2 =vect_pscal_sign(&v, &v1); - - v.x = p.x - zones[j].p.x; - v.y = p.y - zones[j].p.y; - - dist = vect_norm(&v); - IMG_DEBUG("dist pt h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist); - - - if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){ - good_zone = 0; - break; - } - - - /* test if zone is far from points*/ - - v1.x = zones[j].p.x - zones[z_n].p.x; - v1.y = zones[j].p.y - zones[z_n].p.y; - - dist = vect_norm(&v1); - IMG_DEBUG("dist pt to z1 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - v2.x = zones[j].p.x - zones[i].p.x; - v2.y = zones[j].p.y - zones[i].p.y; - - - dist = vect_norm(&v2); - IMG_DEBUG("dist pt to z2 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - - - z1 = i; - z2 = z_n; - z3 = j; - - - - - - /* - XXX may be a lintel on lintel !! - */ - - } - - if (!good_zone) - continue; - - IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - sisters[z_n][current_sister] = i; - current_sister++; - } - } -} - -/* test if a higher zone is too close */ -int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n) -{ - uint8_t i; - vect_t v; - double dist; - - for (i = 0; i < zones_num; i++){ - if (i == z_n) - continue; - if (zones[i].h <= zones[z_n].h) - continue; - - v.x = zones[i].p.x - zones[z_n].p.x; - v.y = zones[i].p.y - zones[z_n].p.y; - - dist = vect_norm(&v); - //IMG_DEBUG("dist pt to pt %2.2f", dist); - - if (dist < OBJECT_DIM){ - return 1; - } - - } - - return 0; -} - -#define MAX_COLUMN 4 -#define MAX_LINTEL 2 - -drop_column_zone drop_c[MAX_COLUMN]; -drop_lintel_zone drop_l[MAX_LINTEL]; - - -void reset_drop_zone(void) -{ - memset(drop_c, 0, sizeof(drop_c)); - memset(drop_l, 0, sizeof(drop_l)); - -} - - -void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones) -{ - unsigned int i; - - for (i=0;i(b)?(a):(b)) - - -#if 0 -#define MAX_DROP_HIGH 8 - - -/* - recursive function to maximize points during object - dropping, given lintel/column number - working zone may be 1, 2 or 3 - */ -unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, - uint8_t n_columns, uint8_t n_lintels, - uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone) -{ - - uint8_t i, j; - unsigned int points_calc; - //unsigned int points_added = 0; - int sister; - int ret; - - - /* if no more objects, return points */ - if (n_columns == 0 && n_lintels == 0) - return MY_MAX(points, points_max); - - /* start by putting columns if so */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h >= MAX_DROP_HIGH) - continue; - - if (n_columns){ - - ret = test_close_zone(zones_num, zones, i); - if (ret) - continue; - - zones[i].h++; - points_calc = solve_objects_dropping(points + zones[i].h, points_max, - n_columns - 1, n_lintels, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - drop_c[n_columns - 1].z = i; - drop_c[n_columns - 1].h = zones[i].h; - drop_c[n_columns - 1].valid = 1; - - } - zones[i].h--; - } - /* we must depose all columns before dropping lintels */ - else if (n_lintels){ - - /* dont drop lintel on ground */ - if (zones[i].h <= working_zone) - continue; - - /* XXX todo need get second zone near selected one */ - //ret = find_twin_tower(zones_num, zones, i, &sister); - - for (j = 0; j < MAX_SISTER_PER_ZONE; j++){ - sister = sisters[i][j]; - if (sister == -1) - break; - if (zones[i].h != zones[sister].h){ - sister = -1; - } - - } - - if (sister == -1) - continue; - //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h); - - zones[i].h++; - zones[sister].h++; - - - points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max, - n_columns, n_lintels - 1, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - - drop_l[n_lintels - 1].z1 = i; - drop_l[n_lintels - 1].z2 = sister; - drop_l[n_lintels - 1].h = zones[i].h; - drop_l[n_lintels - 1].valid = 1; - - } - - - zones[sister].h--; - zones[i].h--; - } - } - - return MY_MAX(points, points_max); -} -#endif - - -/* */ -int find_column_dropzone(uint8_t zones_num, zone* zones) -{ - uint8_t i; - - uint8_t z_n = 0; - - if (zones_num <= 0) - return -1; - - for (i = 0; i < zones_num; i++){ - if (!zones[i].valid) - continue; - if (zones[i].h > zones[z_n].h) - z_n = i; - } - - - /* - now, chose dropzone closest to robot - meaning little x, big y - so maximise y-x - */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h != zones[z_n].h) - continue; - if (!zones[i].valid) - continue; - if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x) - z_n = i; - } - - - - return z_n; -} - - - -uint8_t color2h(uint8_t color) -{ - return (0x100-color)/0x20; -} - -uint8_t h2color(uint8_t color) -{ - return color*0x20; -} - - -#define NUM_ZONE_GENERATE 8 -#define DIST_ZONE_GEN 9 - -/* - remove zone at ground level, and generate zones on - a circle at X cm from center - */ -unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y) -{ - double c_a, s_a; - uint8_t i, j; - double px1, py1, px2, py2; - - - /* first del zone at level 2 */ - for (i = 0; i < zones_num; ){ - if (zones[i].h!=2){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - - } - - /* generate zones around circle */ - - c_a = cos(2*M_PI/NUM_ZONE_GENERATE); - s_a = sin(2*M_PI/NUM_ZONE_GENERATE); - - px1 = DIST_ZONE_GEN; - py1 = 0; - - for (i = 0; i < NUM_ZONE_GENERATE; i++){ - - zones[zones_num].p.x = center_x + px1; - zones[zones_num].p.y = center_y + py1; - zones[zones_num].h = 2; - zones[zones_num].valid = 1; - - - px2 = px1*c_a + py1*s_a; - py2 = -px1*s_a + py1*c_a; - - px1 = px2; - py1 = py2; - - /* skip zone if it is not in img */ - if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 || - zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in) - continue; - - /* skip zone if not enougth pixels */ - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - - zones_num++; - if (zones_num >= max_zones) - break; - - } - - return zones_num; - - -} - - -/* - remove zone at ground level, and generate zones on - a line at X cm from robot - */ -unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t y; - - /* first del zone at level i */ - for (i = 0; i < zones_num; ){ - if (zones[i].h != working_zone ){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - } - - - /* generate zones on a line */ - for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){ - - zones[zones_num].p.x = center_x; - zones[zones_num].p.y = y; - zones[zones_num].h = working_zone ; - zones[zones_num].valid = 1; - - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - zones_num++; - - if (zones_num >= max_zones) - break; - - } - return zones_num; - -} - - -#define MAX_DECAL_LINE 5 -#define ENOUGHT_ZONE_PIXEL 2 - -void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t cpt; - - /* recal img only for central zone */ - if (working_zone !=2) - return; - - for (i = 0; i < MAX_DECAL_LINE; i++){ - cpt = 0; - for (j = 0; j < x_in; j++){ - if (buffer[i*x_in + j] ==2) - cpt++; - } - - if (cpt >= ENOUGHT_ZONE_PIXEL) - break; - } - - memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in); - memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in); -} - - -#define MAX_OBJECTS 20 - -#define MAX_ZONES_PER_OBJECT 20 - - -uint8_t g_zone_num; -zone g_all_zones[MAX_ZONES]; - -uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, - zone * all_zones, uint8_t max_zones) -{ - - int ret; - int i, j; - - zone zones[MAX_ZONES_PER_OBJECT]; - - vect_t caliper; - unsigned int pts_cal[4]; - point_t ptmp; - vect_t r1, r2; - int zone_len; - - - uint8_t zone_num = 0; - - Object_bb sac_obj[MAX_OBJECTS]; - Object_poly sac_obj_poly[MAX_OBJECTS]; - - - - /* - XXX fix: only decal for working zone 2/(1?) - but we dont have info yet - */ - recal_img_y(buffer, x_in, y_in, 2); - - memset(sac_obj, 0, sizeof(sac_obj)); - memset(sac_obj_poly, 0, sizeof(sac_obj_poly)); - - - /* first, find polygons*/ - parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS); - - /* enclose each poygon in the min area polygon - then, split each rectangle in dropping zone - */ - for (i=0;i 0; i--){ - z.h = i; - z.valid = 1; - ret = zone_has_enought_pixels(buffer, x_in, y_in, &z); - IMG_NOTICE("test z3:(h=%d) %d", i, ret); - - if (ret) - return 1; - } - - return 0; - -} - - -int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y) -{ - uint8_t zone_num; - int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE]; - int8_t z_n = -1; - uint8_t i, j; - - - /* find all drop zone */ - zone_num = filter_zones(buffer, x_in, y_in, - g_all_zones, g_zone_num, MAX_ZONES, - working_zone, center_x, center_y, - -2); - - /* precompute possible twin towers */ - find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y); - - - for (i=0; i< zone_num; i++){ - IMG_DEBUG("all sisters: %d", i); - for (j=0;j g_all_zones[i].h) - continue; - - z_n = i; - } - - IMG_NOTICE("twin tower found :z_n=%d", z_n); - if (z_n == -1) - return -1; - - IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - g_all_zones[z_n].p.x, g_all_zones[z_n].p.y, - g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y); - - - *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2; - *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30; - - - return g_all_zones[z_n].h; - -} -