5 #include <aversive/pgmspace.h>
6 #include <aversive/error.h>
14 #include <vect_base.h>
18 #include "img_processing.h"
21 #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
30 #include <blocking_detection_manager.h>
33 #include <control_system_manager.h>
40 #define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args)
41 #define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args)
42 #define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args)
46 #define IMG_DEBUG(args...) debug_printf(args)
47 #define IMG_NOTICE(args...) debug_printf(args)
48 #define IMG_ERROR(args...) debug_printf(args)
55 #define OBJECT_MINIMUM_DEMI_PERIMETER (2*3)
56 /* store object in pool if never seen
61 int store_obj(Object_bb* tab_o, int total, Object_bb*o)
65 if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER)
68 for (i=0;i<total;i++){
69 if (!memcmp(&tab_o[i], o, sizeof(Object_bb)))
72 if (tab_o[i].x_min==0 && tab_o[i].x_max==0 &&
73 tab_o[i].y_min==0 && tab_o[i].y_max==0){
74 memcpy(&tab_o[i], o, sizeof(Object_bb));
82 /* step around object of given color and compute its bounding box */
83 void object_find_bounding_box(unsigned char* data,
84 int16_t x_in, int16_t y_in,
85 int16_t start_x, int16_t start_y,
86 int16_t color, int16_t color_w,Object_bb* o)
88 int16_t pos_x = start_x;
89 int16_t pos_y = start_y;
92 int16_t count_stand = 0;
109 if (pos_x == start_x && pos_y == start_y){
113 if( v.x == vi.x && v.y == vi.y && start)
126 /* is next pixel is good color */
127 if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
145 /* step around object of given color and computes its polygon */
146 void object_find_poly(unsigned char* data,
147 int16_t x_in, int16_t y_in,
148 int16_t start_x, int16_t start_y,
149 int16_t color, int16_t color_w, Object_poly* o)
151 int16_t pos_x = start_x;
152 int16_t pos_y = start_y;
155 int16_t count_stand = 0;
156 uint16_t pt_step, pt_num;
164 pt_step = o->len/POLY_MAX_PTS + 1;
169 if (pos_x == start_x && pos_y == start_y){
173 if( v.x == vi.x && v.y == vi.y && start)
177 /* is next pixel is good color */
178 if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
183 if (len >pt_num*pt_step){
184 o->pts[pt_num].x = pos_x;
185 o->pts[pt_num].y = pos_y;
187 if (pt_num>=POLY_MAX_PTS)
207 /* return most left/right/top/down pts indexes of given polygon */
208 void object_find_extrem_points_index(Object_poly* o,
216 for (i=1;i<o->pts_num;i++){
217 if (o->pts[i].x < o->pts[pts[PT_LEFT]].x)
219 if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x)
221 if (o->pts[i].y < o->pts[pts[PT_TOP]].y)
223 if (o->pts[i].y > o->pts[pts[PT_DOWN]].y)
230 #define NUM_CALIPERS 4
231 /* for debug purpose: display a vector on image */
232 void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in,
233 vect_t *v, point_t p)
249 x+=(float)((v->x)*(float)i*(float)coef);
250 y+=(float)((v->y)*(float)i*(float)coef);
251 if ((x== p.x) && (y == p.y))
263 /* compute minimum rectangle area including the given convex polygon */
264 void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out,
265 vect_t *v_out, vect_t *r1, vect_t*r2)
267 vect_t calipers[NUM_CALIPERS];
268 vect_t edges[NUM_CALIPERS];
271 unsigned int calipers_pts_index[NUM_CALIPERS];
272 float angles[NUM_CALIPERS];
274 float total_rot_angle = 0;
275 int caliper_result_index;
278 float ps, n1, n2, caliper_n;
281 /* XXX hack sould be max*/
282 float aera_min=0x100000;
284 object_find_extrem_points_index(o, calipers_pts_index);
299 while (total_rot_angle <= M_PI/2){
301 for (i=0;i<NUM_CALIPERS;i++){
302 /* compute polygon edge vector */
303 edges[i].x = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].x -
304 o->pts[calipers_pts_index[i]].x;
305 edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y -
306 o->pts[calipers_pts_index[i]].y;
308 /* compute angle between caliper and polygon edge */
309 angles[i] = vect_get_angle(&edges[i], &calipers[i]);
313 min_angle = angles[0];
314 caliper_result_index = 0;
315 for (i=1;i<NUM_CALIPERS;i++){
316 if (angles[i]<min_angle){
317 min_angle = angles[i];
318 caliper_result_index = i;
322 /* rotate calipers */
323 calipers[caliper_result_index] = edges[caliper_result_index];
325 for (i=caliper_result_index; i<caliper_result_index + o->pts_num; i++){
326 calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS];
327 vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]);
330 /* update calipers point */
331 for (i=0;i<NUM_CALIPERS;i++){
332 if (angles[i]==min_angle)
333 calipers_pts_index[i] = (calipers_pts_index[i] + 1) % o->pts_num;
336 res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x;
337 res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y;
339 res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x;
340 res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y;
342 ps = vect_pscal(&res1, &calipers[CAL_X]);
343 n1 = vect_norm(&res1);
344 caliper_n = vect_norm(&calipers[CAL_X]);
345 caliper_n*=caliper_n;
347 res1 = calipers[CAL_X];
349 res1.x *= ps/(caliper_n);
350 res1.y *= ps/(caliper_n);
353 ps = vect_pscal(&res2, &calipers[CAL_Y]);
354 n1 = vect_norm(&res2);
356 res2 = calipers[CAL_Y];
358 res2.x *= ps/(caliper_n);
359 res2.y *= ps/(caliper_n);
361 n1 = vect_norm(&res1);
362 n2 = vect_norm(&res2);
366 if (aera_min >aera_tmp){
368 for (i=0;i<NUM_CALIPERS;i++){
369 pts_index_out[i] = calipers_pts_index[i];
371 *v_out = calipers[0];
376 total_rot_angle+=min_angle;
383 /* transform caliper to rectangle coordinates */
384 int object_poly_caliper_to_rectangle(Object_poly *o,
385 unsigned int *pts_index_out, vect_t* caliper,
386 vect_t *r1, vect_t*r2, point_t *p)
392 point_t p_int1;//, p_int2;
396 caliper_tmp = *caliper;
398 //IMG_DEBUG("cal: %" PRIi32 " %" PRIi32 "", caliper_tmp.x, caliper_tmp.y);
403 /* to be precise, calc 4 intersection of 4 calipers */
404 for (i=0;i<NUM_CALIPERS;i++){
405 p1.x = o->pts[pts_index_out[i]].x;
406 p1.y = o->pts[pts_index_out[i]].y;
408 p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
409 p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
411 pts2line(&p1, &p2, &l1);
413 vect_rot_trigo(&caliper_tmp);
415 p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x;
416 p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y;
418 p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
419 p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
421 pts2line(&p1, &p2, &l2);
423 ret = intersect_line(&l1, &l2, &p_int1);
426 //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y);
435 p->x = lround(mp_x/NUM_CALIPERS);
436 p->y = lround(mp_y/NUM_CALIPERS);
444 /* split zone in many column's sized area */
445 int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color)
457 r1_s = n1/OBJECT_DIM;
458 r2_s = n2/OBJECT_DIM;
463 ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2));
464 ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2));
468 zones[index].p.x = ptmp.x + (i*r1->x)/r1_s+(j*r2->x)/r2_s;
469 zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s;
470 zones[index].h = color;
471 zones[index].valid = 1;
484 #define OBJECT_SEMI_DIM (OBJECT_DIM/2)
485 #define MIN_SURFACE_PERCENT 50
486 #define HIGHER_MAX_PIXEL 5
489 int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z)
492 uint16_t count, total_pix, higher_pixels;
498 for (x = -OBJECT_SEMI_DIM;
499 (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
501 for (y = -OBJECT_SEMI_DIM;
502 (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
505 if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h)
508 if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h)
515 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,
516 count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels);
518 if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) &&
519 (higher_pixels <HIGHER_MAX_PIXEL))
525 int zone_filter_min_surface(unsigned char* data, int16_t x_in, int16_t y_in,
526 uint8_t color, unsigned int zones_num, zone* p)
530 for (i = 0; i < zones_num ; i++){
531 if (zone_has_enought_pixels(data, x_in, y_in, &p[i]))
537 IMG_NOTICE("num zone after min surf: %d", zones_num);
543 /* center is 15 cm radius*/
544 #define CENTER_RADIUS 15
546 /* the complete column must be in the drop zone*/
547 #define CENTER_MAX_DIST (15-3)
549 /* the column must not be too close from center*/
550 #define CENTER_MIN_DIST (8)
552 int zone_filter_center(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y, int tweak_min_margin)
557 for (i = 0; i < zones_num; i++){
559 v.x = p[i].p.x - center_x;
560 v.y = p[i].p.y - center_y;
561 IMG_DEBUG("square dist to center %"PRIi32" (%d %d)",
562 v.x*v.x + v.y*v.y, (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin), CENTER_MAX_DIST * CENTER_MAX_DIST);
564 if (v.x*v.x + v.y*v.y < CENTER_MAX_DIST * CENTER_MAX_DIST &&
565 v.x*v.x + v.y*v.y > (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin))
575 #define MAX_DIST_TO_ZONE 2
577 unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone)
581 for (i = 0; i < zones_num; i++){
583 IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x);
585 if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone))
595 /* delete point to render polygon convex */
596 int object_poly_to_convex(Object_poly *o)
601 unsigned int del_pts_num = 0;
603 for (i=0;i<o->pts_num;){
604 v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x;
605 v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y;
607 w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x;
608 w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y;
610 z = vect_pvect(&v, &w);
616 /* found a convex angle (or colinear points) */
617 for (j = i; j < o->pts_num-1; j++){
618 o->pts[j] = o->pts[j+1];
633 #define DEFAULT_COLOR 255
634 /* scan all image and find objects*/
635 unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in,
636 Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj)
648 first, delete borders
650 for (i=0;i<x_in;i++){
652 data[(y_in - 1) * x_in + i] = 0;
655 for (i=0;i<y_in;i++){
657 data[i * x_in + x_in - 1] = 0;
664 0 look for color (object or edge)
668 for (i=1;i<x_in*y_in;i++){
685 // we found an object
686 object_find_bounding_box(data, x_in, y_in, (i-1)%x_in, (i-1)/x_in, data[i], 255, &o);
688 ret = store_obj(sac_obj, max_obj, &o);
689 /* if new object, process rotating calipers */
691 sac_obj_poly[obj_num].len = o.len;
692 object_find_poly(data, x_in, y_in,
693 (i-1)%x_in, (i-1)/x_in,
694 data[i], 255, &sac_obj_poly[obj_num]);
695 IMG_DEBUG("%d",sac_obj_poly[obj_num].pts_num);
696 object_poly_to_convex(&sac_obj_poly[obj_num]);
698 for (j=0;j<sac_obj_poly[obj_num].pts_num;j++){
699 data[sac_obj_poly[obj_num].pts[j].y*x_in + sac_obj_poly[obj_num].pts[j].x] = 8;
701 sac_obj_poly[obj_num].color = data[i];
708 //we must out of color
714 if (data[i] != in_color){
719 we are in a color and want to go out of it
734 /* space between twin tower is 13 pixels*/
735 #define SPACE_INTER_TWIN_TOWER (13)
737 #define SPACE_INTER_TWIN_TOWER_TOLERENCE 3
741 /* find best twin tower for each zone */
742 void find_twin_tower(uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE],
743 int16_t center_x, int16_t center_y)
749 unsigned int z1, z2, z3;
750 //int32_t scal1, scal2;
754 unsigned int good_zone;
756 unsigned int current_sister;
758 /* init dummy sisters */
759 for (i = 0; i < zones_num; i++)
760 for (j = 0; j < MAX_SISTER_PER_ZONE; j++)
765 for (z_n = 0; z_n < zones_num; z_n++){
766 if (!zones[z_n].valid)
771 for (i = 0; i < zones_num; i++){
774 /* we already have max sisters */
775 if (current_sister >= MAX_SISTER_PER_ZONE)
786 /* twin tower must have same high */
787 if (zones[i].h != zones[z_n].h)
790 IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
791 zones[z_n].p.x, zones[z_n].p.y,
792 zones[i].p.x, zones[i].p.y);
795 dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) +
796 (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) );
799 IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER);
802 twin tower must be close/far enought to drop lintel
804 if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE)
808 pts2line(&zones[i].p, &zones[z_n].p, &l);
811 test the paralelism of the temple:
812 zone may be on same distance from center
816 v1.x = zones[z_n].p.x - center_x;
817 v1.y = zones[z_n].p.y - center_y;
819 dist = vect_norm(&v1);
821 v2.x = zones[i].p.x - center_x;
822 v2.y = zones[i].p.y - center_y;
824 dist2 = vect_norm(&v2);
826 IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2);
827 if (ABS(dist-dist2) > 3){
828 IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2));
835 /* no other aligned tower to avoid dropping on a lintel
836 * (3 aligned zone may mean lintel)
841 for (j = 0; j < zones_num; j++){
845 /* if third zone, but lower */
846 if (zones[j].h <= zones[i].h)
850 check distance from dropping zone to
851 line formed by twin towers
854 proj_pt_line(&zones[j].p, &l, &p);
857 /* test if projected point is in the segement */
860 v.x = zones[z_n].p.x - zones[i].p.x;
861 v.y = zones[z_n].p.y - zones[i].p.y;
863 v1.x = p.x - zones[i].p.x;
864 v1.y = p.y - zones[i].p.y;
866 n1 = vect_pscal_sign(&v, &v1);
871 v1.x = p.x - zones[z_n].p.x;
872 v1.y = p.y - zones[z_n].p.y;
875 n2 =vect_pscal_sign(&v, &v1);
877 v.x = p.x - zones[j].p.x;
878 v.y = p.y - zones[j].p.y;
880 dist = vect_norm(&v);
881 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);
884 if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){
890 /* test if zone is far from points*/
892 v1.x = zones[j].p.x - zones[z_n].p.x;
893 v1.y = zones[j].p.y - zones[z_n].p.y;
895 dist = vect_norm(&v1);
896 IMG_DEBUG("dist pt to z1 %2.2f", dist);
898 if (dist < OBJECT_DIM){
903 v2.x = zones[j].p.x - zones[i].p.x;
904 v2.y = zones[j].p.y - zones[i].p.y;
907 dist = vect_norm(&v2);
908 IMG_DEBUG("dist pt to z2 %2.2f", dist);
910 if (dist < OBJECT_DIM){
926 XXX may be a lintel on lintel !!
934 IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
935 zones[z_n].p.x, zones[z_n].p.y,
936 zones[i].p.x, zones[i].p.y);
939 sisters[z_n][current_sister] = i;
945 /* test if a higher zone is too close */
946 int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n)
952 for (i = 0; i < zones_num; i++){
955 if (zones[i].h <= zones[z_n].h)
958 v.x = zones[i].p.x - zones[z_n].p.x;
959 v.y = zones[i].p.y - zones[z_n].p.y;
961 dist = vect_norm(&v);
962 //IMG_DEBUG("dist pt to pt %2.2f", dist);
964 if (dist < OBJECT_DIM){
976 drop_column_zone drop_c[MAX_COLUMN];
977 drop_lintel_zone drop_l[MAX_LINTEL];
980 void reset_drop_zone(void)
982 memset(drop_c, 0, sizeof(drop_c));
983 memset(drop_l, 0, sizeof(drop_l));
988 void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones)
992 for (i=0;i<n_columns;i++)
993 IMG_NOTICE("c %d:(h:%d) (%"PRIi32" %"PRIi32") valid=%d",
995 zones[drop_c[i].z].p.x, zones[drop_c[i].z].p.y,
998 for (i=0;i<n_lintels;i++)
999 IMG_NOTICE("l %d:(h:%d) (%"PRIi32" %"PRIi32") "
1000 "(%"PRIi32" %"PRIi32") valid=%d",
1002 zones[drop_l[i].z1].p.x, zones[drop_l[i].z1].p.y,
1003 zones[drop_l[i].z2].p.x, zones[drop_l[i].z2].p.y, drop_l[i].valid);
1007 #define MY_MAX(a, b) ((a)>(b)?(a):(b))
1011 #define MAX_DROP_HIGH 8
1015 recursive function to maximize points during object
1016 dropping, given lintel/column number
1017 working zone may be 1, 2 or 3
1019 unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max,
1020 uint8_t n_columns, uint8_t n_lintels,
1021 uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone)
1025 unsigned int points_calc;
1026 //unsigned int points_added = 0;
1031 /* if no more objects, return points */
1032 if (n_columns == 0 && n_lintels == 0)
1033 return MY_MAX(points, points_max);
1035 /* start by putting columns if so */
1036 for (i = 0; i < zones_num; i++){
1037 if (zones[i].h >= MAX_DROP_HIGH)
1042 ret = test_close_zone(zones_num, zones, i);
1047 points_calc = solve_objects_dropping(points + zones[i].h, points_max,
1048 n_columns - 1, n_lintels,
1049 zones_num, zones, sisters, working_zone);
1051 if (points_calc > points_max){
1052 points_max = points_calc;
1053 drop_c[n_columns - 1].z = i;
1054 drop_c[n_columns - 1].h = zones[i].h;
1055 drop_c[n_columns - 1].valid = 1;
1060 /* we must depose all columns before dropping lintels */
1061 else if (n_lintels){
1063 /* dont drop lintel on ground */
1064 if (zones[i].h <= working_zone)
1067 /* XXX todo need get second zone near selected one */
1068 //ret = find_twin_tower(zones_num, zones, i, &sister);
1070 for (j = 0; j < MAX_SISTER_PER_ZONE; j++){
1071 sister = sisters[i][j];
1074 if (zones[i].h != zones[sister].h){
1082 //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h);
1088 points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max,
1089 n_columns, n_lintels - 1,
1090 zones_num, zones, sisters, working_zone);
1092 if (points_calc > points_max){
1093 points_max = points_calc;
1095 drop_l[n_lintels - 1].z1 = i;
1096 drop_l[n_lintels - 1].z2 = sister;
1097 drop_l[n_lintels - 1].h = zones[i].h;
1098 drop_l[n_lintels - 1].valid = 1;
1108 return MY_MAX(points, points_max);
1114 int find_column_dropzone(uint8_t zones_num, zone* zones)
1123 for (i = 0; i < zones_num; i++){
1124 if (!zones[i].valid)
1126 if (zones[i].h > zones[z_n].h)
1132 now, chose dropzone closest to robot
1133 meaning little x, big y
1136 for (i = 0; i < zones_num; i++){
1137 if (zones[i].h != zones[z_n].h)
1139 if (!zones[i].valid)
1141 if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x)
1152 uint8_t color2h(uint8_t color)
1154 return (0x100-color)/0x20;
1157 uint8_t h2color(uint8_t color)
1163 #define NUM_ZONE_GENERATE 8
1164 #define DIST_ZONE_GEN 9
1167 remove zone at ground level, and generate zones on
1168 a circle at X cm from center
1170 unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
1171 zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y)
1175 double px1, py1, px2, py2;
1178 /* first del zone at level 2 */
1179 for (i = 0; i < zones_num; ){
1185 for (j = i; j < zones_num-1; j++)
1186 zones[j] = zones[j+1];
1192 /* generate zones around circle */
1194 c_a = cos(2*M_PI/NUM_ZONE_GENERATE);
1195 s_a = sin(2*M_PI/NUM_ZONE_GENERATE);
1197 px1 = DIST_ZONE_GEN;
1200 for (i = 0; i < NUM_ZONE_GENERATE; i++){
1202 zones[zones_num].p.x = center_x + px1;
1203 zones[zones_num].p.y = center_y + py1;
1204 zones[zones_num].h = 2;
1205 zones[zones_num].valid = 1;
1208 px2 = px1*c_a + py1*s_a;
1209 py2 = -px1*s_a + py1*c_a;
1214 /* skip zone if it is not in img */
1215 if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 ||
1216 zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in)
1219 /* skip zone if not enougth pixels */
1220 if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
1224 if (zones_num >= max_zones)
1236 remove zone at ground level, and generate zones on
1237 a line at X cm from robot
1239 unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
1240 zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y,
1241 uint8_t working_zone)
1246 /* first del zone at level i */
1247 for (i = 0; i < zones_num; ){
1248 if (zones[i].h != working_zone ){
1253 for (j = i; j < zones_num-1; j++)
1254 zones[j] = zones[j+1];
1260 /* generate zones on a line */
1261 for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){
1263 zones[zones_num].p.x = center_x;
1264 zones[zones_num].p.y = y;
1265 zones[zones_num].h = working_zone ;
1266 zones[zones_num].valid = 1;
1268 if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
1272 if (zones_num >= max_zones)
1281 #define MAX_DECAL_LINE 5
1282 #define ENOUGHT_ZONE_PIXEL 2
1284 void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in,
1285 uint8_t working_zone)
1290 /* recal img only for central zone */
1291 if (working_zone !=2)
1294 for (i = 0; i < MAX_DECAL_LINE; i++){
1296 for (j = 0; j < x_in; j++){
1297 if (buffer[i*x_in + j] ==2)
1301 if (cpt >= ENOUGHT_ZONE_PIXEL)
1305 memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in);
1306 memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in);
1310 #define MAX_OBJECTS 20
1312 #define MAX_ZONES_PER_OBJECT 20
1316 zone g_all_zones[MAX_ZONES];
1318 uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in,
1319 zone * all_zones, uint8_t max_zones)
1325 zone zones[MAX_ZONES_PER_OBJECT];
1328 unsigned int pts_cal[4];
1334 uint8_t zone_num = 0;
1336 Object_bb sac_obj[MAX_OBJECTS];
1337 Object_poly sac_obj_poly[MAX_OBJECTS];
1342 XXX fix: only decal for working zone 2/(1?)
1343 but we dont have info yet
1345 recal_img_y(buffer, x_in, y_in, 2);
1347 memset(sac_obj, 0, sizeof(sac_obj));
1348 memset(sac_obj_poly, 0, sizeof(sac_obj_poly));
1351 /* first, find polygons*/
1352 parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS);
1354 /* enclose each poygon in the min area polygon
1355 then, split each rectangle in dropping zone
1357 for (i=0;i<MAX_OBJECTS;i++){
1359 if (!sac_obj_poly[i].pts_num)
1362 IMG_DEBUG("obj: %d %d %d %d %d",
1369 //IMG_DEBUG("poly pts_num: %d", sac_obj_poly[i].pts_num);
1371 object_poly_get_min_ar(&sac_obj_poly[i], &pts_cal[0], &caliper, &r1, &r2);
1373 ret = object_poly_caliper_to_rectangle(&sac_obj_poly[i], &pts_cal[0], &caliper,
1380 IMG_DEBUG("r: (%3"PRIi32" %3"PRIi32") "
1381 "(%3"PRIi32" %3"PRIi32")",
1382 r1.x, r1.y, r2.x, r2.y);
1383 IMG_DEBUG("intersection: %"PRIi32" %"PRIi32"",
1387 zone_len = split_rectangle(&ptmp, &r1, &r2,
1388 MAX_ZONES_PER_OBJECT, &zones[0], sac_obj_poly[i].color);
1389 //IMG_DEBUG("split ok %d", zone_len);
1391 zone_len = zone_filter_min_surface(buffer, x_in, y_in,
1392 sac_obj_poly[i].color,
1393 zone_len, &zones[0]);
1395 for (j = 0; j < zone_len && zone_num < max_zones; zone_num++, j++)
1396 all_zones[zone_num] = zones[j];
1401 IMG_NOTICE("num zones end: %d", zone_num);
1407 void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in)
1409 g_zone_num = process_img(buffer, x_in, y_in,
1410 g_all_zones, MAX_ZONES);
1413 uint8_t filter_zones(unsigned char *buffer, int16_t x_in, int16_t y_in,
1414 zone * all_zones, uint8_t zone_num, uint8_t max_zones,
1415 uint8_t working_zone, int16_t center_x, int16_t center_y,
1416 int tweak_min_margin)
1420 /* first valid all zones */
1421 for (i = 0; i < zone_num; i++){
1422 all_zones[i].valid = 1;
1424 /* filter zone lower thatn working zone */
1425 if (all_zones[i].h < working_zone)
1426 all_zones[i].valid = 0;
1431 generate correct zone at ground level
1432 (depending on working zone)
1434 if (working_zone == 2)
1435 zone_num = generate_center_ground_zones(buffer, x_in, y_in,
1436 all_zones, zone_num, max_zones, center_x, center_y);
1438 zone_num = generate_rectangle_ground_zones(buffer, x_in, y_in,
1439 all_zones, zone_num, max_zones, center_x, center_y,
1442 /* filter zone position, depending on workingzone */
1443 if (working_zone == 2)
1444 zone_num = zone_filter_center(zone_num, all_zones, center_x, center_y, tweak_min_margin);
1446 zone_num = zone_filter_zone_rect(zone_num, all_zones, center_x, center_y , working_zone);
1450 /* display zones (debug purpose) */
1452 for (i = 0; i < zone_num; i++){
1453 //buffer[all_zones[i].p.y*x_in+all_zones[i].p.x] = 0x3;
1454 IMG_NOTICE("h:%d (v:%d) x:%"PRIi32" y:%"PRIi32"", all_zones[i].h, all_zones[i].valid, all_zones[i].p.x, all_zones[i].p.y);
1458 IMG_NOTICE("num zones: %d", zone_num);
1468 return -1 if not column dropzone is found
1469 return column hight if found
1471 int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
1472 uint8_t working_zone, int16_t center_x, int16_t center_y,
1473 int16_t * dropzone_x, int16_t * dropzone_y)
1480 zone_num = filter_zones(buffer, x_in, y_in,
1481 g_all_zones, g_zone_num, MAX_ZONES,
1482 working_zone, center_x, center_y,
1485 c_drop_zone = find_column_dropzone(zone_num, g_all_zones);
1490 *dropzone_x = g_all_zones[c_drop_zone].p.x;
1491 *dropzone_y = g_all_zones[c_drop_zone].p.y;
1493 *dropzone_x = (*dropzone_x) * PIXEL2CM;
1494 *dropzone_y = (*dropzone_y) * PIXEL2CM + 30;
1496 return g_all_zones[c_drop_zone].h;
1499 #define ROBOT_SEMI_INTERCOLUMN_SPACE 75
1501 uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in,
1502 uint8_t h, int16_t center_x, int16_t center_y)
1509 IMG_NOTICE("test z (mm) : x:%d y:%d", center_x, center_y);
1510 IMG_NOTICE("test z:(pix): x:%d y:%d", (int)(center_x/ PIXEL2CM), (int)(center_y/ PIXEL2CM));
1513 z.p.x = center_x / PIXEL2CM;
1514 z.p.y = (center_y - ROBOT_SEMI_INTERCOLUMN_SPACE) / PIXEL2CM ;
1517 ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1518 IMG_NOTICE("test z1: %d", ret);
1523 z.p.x = center_x / PIXEL2CM;
1524 z.p.y = (center_y + ROBOT_SEMI_INTERCOLUMN_SPACE)/ PIXEL2CM;
1527 ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1528 IMG_NOTICE("test z2: %d", ret);
1534 if middle zone is less or egual to tested temple
1536 z.p.x = center_x / PIXEL2CM;
1537 z.p.y = center_y / PIXEL2CM;
1539 for (i = h; i > 0; i--){
1542 ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1543 IMG_NOTICE("test z3:(h=%d) %d", i, ret);
1554 int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
1555 uint8_t working_zone, int16_t center_x, int16_t center_y,
1556 int16_t * dropzone_x, int16_t * dropzone_y)
1559 int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE];
1564 /* find all drop zone */
1565 zone_num = filter_zones(buffer, x_in, y_in,
1566 g_all_zones, g_zone_num, MAX_ZONES,
1567 working_zone, center_x, center_y,
1570 /* precompute possible twin towers */
1571 find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y);
1574 for (i=0; i< zone_num; i++){
1575 IMG_DEBUG("all sisters: %d", i);
1576 for (j=0;j<MAX_SISTER_PER_ZONE;j++){
1577 IMG_DEBUG("s: %d", sisters[i][j]);
1581 /* only use first sister of each zone */
1582 for (i=0; i< zone_num; i++){
1584 /* if zone doesn't have twin tower */
1585 if (sisters[i][0] == -1)
1588 if (!g_all_zones[i].valid)
1596 /* if we found higher twin tower */
1597 if (g_all_zones[z_n].h > g_all_zones[i].h)
1603 IMG_NOTICE("twin tower found :z_n=%d", z_n);
1607 IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
1608 g_all_zones[z_n].p.x, g_all_zones[z_n].p.y,
1609 g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y);
1612 *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2;
1613 *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30;
1616 return g_all_zones[z_n].h;