#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /***************** Edge detector switches ************************/ /* Change line below to "#if 1" to test Canny edge detector */ #if 1 #define TEST_CANNY #endif /* Change line below to "#if 1" to test Phil Palmer's edge detector */ #if 0 #define TEST_ES #endif /***************** Line detector switches ************************/ /* Change line below to "#if 1" to test orthogonal regression line detector */ #if 1 #define TEST_ORTHOG #endif /* Change line below to "#if 1" to test Phil Palmer's line detector */ #if 0 #define TEST_HT #endif /***************** Junction detector switches ************************/ /* Change line below to "#if 1" to test Phil Palmer's junction finder */ #if 1 #define TEST_JF #endif /***************** End of algorithm switches ************************/ /***************** Camera calibration stuff *************************/ #define MAX_POINTS 48 typedef struct Vector2 { hor_real x; hor_real y; } Vector2; typedef struct Vector3 { hor_real x; hor_real y; hor_real z; } Vector3; int num_world_points; Vector3 world_points[MAX_POINTS]; typedef struct Mapping { Hor_Bool mapped; /* TRUE if this point corresponds to a junction in the image */ Vector2 image_point; } Mapping; Mapping mappings[MAX_POINTS]; int num_mapped_points; hor_real x_offset = 0.0; hor_real y_offset = 0.0; #define K (0.707106781186548) /***** Function prototypes *****/ void find_image_center(); void print_world_points(); int num_mappings(); /***************** End Camera calibration stuff *********************/ #if 0 #define SIMULATE_IMAGE #endif #if 1 #define FLOAT_IMAGE #endif #ifdef SIMULATE_IMAGE #define IMWIDTH 64 #define IMHEIGHT 64 static Hor_Image *square(void) { Hor_Image *img = hor_im_alloc ( HOR_UCHAR, HOR_INTENSITY, IMHEIGHT, IMWIDTH, IMWIDTH ); int i, j; hor_im_fill_intensity_uc ( img, 0 ); for ( i = IMHEIGHT/6; i < 5*IMHEIGHT/6; i++ ) for ( j = IMWIDTH/6; j < 5*IMHEIGHT/6; j++ ) hor_im_set_uc ( img, i, j, 50 ); hor_im_set_uc ( img, IMHEIGHT/2, IMWIDTH/2, 0 ); hor_im_write ( "sim.000.pgm", HOR_PGM, img ); return img; } #endif #ifdef FLOAT_IMAGE static Hor_Image_Type imtype = HOR_IM_REAL; #else static Hor_Image_Type imtype = HOR_IM_INT; #endif static Hor_Image *convert_image ( Hor_Image *img, Hor_Image_Type type ) { Hor_Image *new_img; int i, j; hor_im_assert_format ( img, HOR_INTENSITY ); hor_im_assert_type ( img, HOR_UCHAR ); new_img = hor_im_alloc ( type, HOR_INTENSITY, img->w.height, img->w.width, img->w.width ); switch ( type ) { case HOR_IM_REAL: for ( i = 0; i < img->w.height; i++ ) for ( j = 0; j < img->w.width; j++ ) new_img->data.intensity.r[i][j] = (hor_real) img->data.intensity.uc[i][j]; break; case HOR_IM_INT: for ( i = 0; i < img->w.height; i++ ) for ( j = 0; j < img->w.width; j++ ) new_img->data.intensity.i[i][j] = img->data.intensity.uc[i][j]; break; default: hor_assert ( 0, "illegal image type in convert_image()" ); break; } return new_img; } #if defined(TEST_CANNY) || defined(TEST_ES) static void edgel_print ( Hor_Edge_Map *edge_map, Hor_Edgel *edgel, void *user_data ) { printf ( "Edgel: r,c=(%d %d) y,x=(%f %f) a=%f str=%f sta=%d\n", edgel->r, edgel->c, edgel->pos.y, edgel->pos.x, edgel->theta, edgel->strength, edgel->status ); } static void hor_edge_map_print ( const char *file_name, Hor_Edge_Map *edge_map ) { FILE *fp; int i; Hor_Edgel *edge; fp = fopen ( file_name, "w" ); if ( fp == NULL ) { perror ( "edge file" ); return; } fprintf ( fp, "%d\n", edge_map->nedges ); for ( i = 0, edge = edge_map->edge; i < edge_map->nedges; i++, edge++ ) fprintf ( fp, "%f %f %f %f\n", edge->pos.x, edge->pos.y, edge->strength, edge->theta ); fclose ( fp ); } #endif #if defined(TEST_ORTHOG) || defined(TEST_HT) static void line_print ( Hor_Line_Map *line_map, Hor_Line *line, void *user_data ) { printf ( "Line: p1=(%f %f) p2=(%f %f) str=%f theta=%f\n", line->pos1.x, line->pos1.y, line->pos2.x, line->pos2.y, line->strength, line->theta ); } #endif #ifdef TEST_JF static void junction_print ( Hor_Junction_Map *junction_map, Hor_Junction *junction, void *user_data ) { int world_point_id; printf ( "Junction(" ); if ( junction->type == HOR_V2_JUNCTION ) printf ( "V2" ); else if ( junction->type == HOR_T_JUNCTION ) printf ( "T" ); else if ( junction->type == HOR_V3_JUNCTION ) printf ( "V3" ); else assert(0); printf ( "): p=(%f,%f) str=%f sta=%d\n", junction->pos.x, junction->pos.y, junction->strength, junction->status ); printf ( " line 1: p1=(%f,%f) p2=(%f,%f)\n", junction->line1->p1.x, junction->line1->p1.y, junction->line1->p2.x, junction->line1->p2.y ); printf ( " line 2: p1=(%f,%f) p2=(%f,%f)\n", junction->line2->p1.x, junction->line2->p1.y, junction->line2->p2.x, junction->line2->p2.y ); if ( junction->type == HOR_V3_JUNCTION ) printf ( " line 3: p1=(%f,%f) p2=(%f,%f)\n", junction->line3->p1.x, junction->line3->p1.y, junction->line3->p2.x, junction->line3->p2.y ); print_world_points(); printf("Enter ID of world point to map to (-1 for no mapping): "); scanf("%d", &world_point_id); /* We map using our translated image coordinate system with the center of the image as 0, and with the positive Y axis facing UP rather than DOWN as normally is the case. */ if(world_point_id >= 0 && world_point_id < num_world_points) { mappings[world_point_id].mapped = HOR_TRUE; mappings[world_point_id].image_point.x = junction->pos.x+x_offset; mappings[world_point_id].image_point.y = (junction->pos.y+y_offset); /*** should we reverse the sign of y? ***/ printf("**** Mapped world point id %d (%f, %f, %f) to image point (%f, %f)\n", world_point_id, world_points[world_point_id].x, world_points[world_point_id].y, world_points[world_point_id].z, mappings[world_point_id].image_point.x, mappings[world_point_id].image_point.y); } } static void juncline_print ( Hor_Junction_Map *junction_map, Hor_JuncLine *line, void *user_data ) { int i; printf ( "Line: p1=(%f,%f) p2=(%f,%f)\n", line->p1.x, line->p1.y, line->p2.x, line->p2.y ); for ( i = line->njuncs-1; i >= 0; i-- ) { Hor_Junction *junction = line->junc[i]; printf ( " Junction(" ); if ( junction->type == HOR_V2_JUNCTION ) printf ( "V2" ); else if ( junction->type == HOR_T_JUNCTION ) printf ( "T" ); else if ( junction->type == HOR_V3_JUNCTION ) printf ( "V3" ); else assert(0); printf ( "): p=(%f,%f) str=%f sta=%d\n", junction->pos.x, junction->pos.y, junction->strength, junction->status ); } } #endif #if defined(TEST_PLESSEY) || defined(TEST_FORSTNER) static void corner_print ( Hor_Corner_Map *corner_map, Hor_Corner *corner, void *user_data ) { printf ( "Corner: r,c=(%d %d) y,x=(%f %f) str=%f sta=%d\n", corner->r, corner->c, corner->pos.y, corner->pos.x, corner->strength, corner->status ); } static void hor_corner_map_print ( const char *file_name, Hor_Corner_Map *corner_map ) { FILE *fp; int i; Hor_Corner *corner; fp = fopen ( file_name, "w" ); if ( fp == NULL ) { perror ( "corner file" ); return; } fprintf ( fp, "%d\n", corner_map->ncorners ); for ( i = 0, corner = corner_map->corner; i < corner_map->ncorners; i++, corner++ ) fprintf ( fp, "%f %f %f\n", corner->pos.x, corner->pos.y, corner->strength ); fclose ( fp ); } #endif #ifdef TEST_CANNY #ifdef FLOAT_IMAGE #define THRESH_SCALE 1.0 #else #define THRESH_SCALE 1000.0 #endif /**************** CANNY EDGE DETECTOR TEST FUNCTION *****************/ static Hor_Edge_Map *canny_test ( Hor_Image *img, Hor_UI_Device device, Hor_UI_Info di ) { Hor_Canny_WorkSpace *wk; Hor_Edge_Map *edge_map; void *mask; Hor_Image *new_img = convert_image ( img, imtype ); /* edge detection parameters */ int max_nedges, max_nstrings; /* Coarse feature map parameters */ int cvs, chs, cvo, cho, max_nindices; /* Canny parameters */ int border_r, border_c, mask_y_size, mask_x_size, string_length_thresh; hor_real sigma, low_thresh, high_thresh; /* display image as background for edges */ hor_ui_image ( di, img, 0, 0, img->w.height, img->w.width ); /* read edge detection parameters */ hor_edge_opt_read ( "test.cfg", "Edge", "", HOR_OPT_ERROR, NULL, NULL, &max_nedges, &max_nstrings, &cvs, &chs, &cvo, &cho, &max_nindices ); /* read edge detection parameters specific to Canny workspace */ hor_canny_wksp_opt_read ( "test.cfg", "Edge", "Canny", HOR_OPT_ERROR, &border_r, &border_c, &mask_y_size, &mask_x_size); /* make sure there is no difference between the axes */ assert ( mask_x_size == mask_y_size ); assert ( border_c == border_r ); /* read edge detection parameters specific to Canny edge detector */ hor_canny_opt_read ( "test.cfg", "Edge", "Canny", HOR_OPT_ERROR, &sigma, &low_thresh, &high_thresh, &string_length_thresh ); /* create convolution mask */ mask = hor_gm_fill1D ( imtype, sigma, mask_x_size, THRESH_SCALE, NULL ); /* create Canny edge detector workspace */ wk = hor_canny_wksp_alloc ( new_img->w.height, new_img->w.width, border_r, border_c, mask_y_size, mask_x_size, new_img->type ); /* create edge map */ edge_map = hor_edge_map_alloc ( new_img->w.height, new_img->w.width, max_nedges, max_nstrings, cvs, chs, cvo, cho, max_nindices ); /* perform Canny edge detection */ hor_canny_find_edges ( new_img, NULL, mask, mask_y_size, mask, mask_x_size, low_thresh*THRESH_SCALE, high_thresh*THRESH_SCALE, string_length_thresh, border_r, border_c, HOR_IMP_LOCAL_TOP_LEFT, edge_map, wk ); /* display edge map */ printf ( "Displaying edges created by Canny edge detector\n" ); hor_edge_map_display ( di, edge_map, HOR_IMP_LOCAL_TOP_LEFT, HOR_GREEN, HOR_BLUE, HOR_YELLOW, HOR_RED, HOR_BLUE ); /* use if(0) here instead of #if 0 so that code will be compiled and will avoid "defined but not used" compiler warning */ if(0) /* write edges to file */ hor_edge_map_print ( "cars.edg", edge_map ); /* wait for a key press in the display window */ printf ( "press any key in the window to continue\n" ); hor_imp_event_wait_va ( device, NULL, di, HOR_EDGE_MAP, edge_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, edgel_print, HOR_FALSE, HOR_RED, HOR_GREEN, NULL ); /* free everything to do with edge detection */ hor_canny_wksp_free ( wk ); hor_free ( mask ); hor_im_free(new_img); return edge_map; } #undef THRESH_SCALE #endif /* #ifdef TEST_CANNY */ #ifdef TEST_ORTHOG /************ ORTHOGONAL REGRESSION LINE DETECTOR TEST FUNCTION *************/ static Hor_Line_Map *orthog_test ( Hor_Edge_Map *edge_map, Hor_Image *img, Hor_UI_Device device, Hor_UI_Info di ) { Hor_Line_Map *line_map; /* Coarse feature map parameters */ int cvs, chs, cvo, cho, max_nindices; /* line detection parameters */ int max_nlines, max_npoints; int min_length, cut_size; int i; hor_real rms_error_thresh; hor_assert ( edge_map != NULL, "need to do some edge detection for orthog_test()" ); /* display image as background for lines */ hor_ui_image ( di, img, 0, 0, img->w.height, img->w.width ); /* read line detection parameters */ hor_line_opt_read ( "test.cfg", "Line", "", HOR_OPT_ERROR, NULL, NULL, &max_nlines, &max_npoints, &cvs, &chs, &cvo, &cho, &max_nindices ); /* read orthogonal regression line detection parameters */ hor_orthog_opt_read ( "test.cfg", "Line", "Orthog", HOR_OPT_ERROR, &min_length, &cut_size, &rms_error_thresh ); /* create line map */ line_map = hor_line_map_alloc ( edge_map->w.height, edge_map->w.width, max_nlines, max_npoints, cvs, chs, cvo, cho, max_nindices ); /* perform line detection using edge map as input */ hor_orthog_find_lines ( edge_map, min_length, cut_size, rms_error_thresh, HOR_IMP_LOCAL_CENTRE, line_map ); /* display line map */ printf("Displaying lines created by orthogonal regression line detector\n"); hor_line_map_display ( di, line_map, HOR_IMP_LOCAL_TOP_LEFT, HOR_GREEN, HOR_BLUE, HOR_YELLOW, HOR_BLUE ); for(i = 0; i < line_map->nlines; i++) { printf("%f\n", line_map->line[i].theta); } /* wait for a key press in the display window */ printf ( "press any key in the window to continue\n" ); hor_imp_event_wait_va ( device, NULL, di, HOR_LINE_MAP, line_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, line_print, HOR_FALSE, HOR_RED, HOR_GREEN, HOR_BLUE, HOR_BLACK, NULL ); return line_map; } #endif /* #ifdef TEST_ORTHOG */ #ifdef TEST_ES /**************** PHIL PALMER'S EDGE DETECTOR TEST FUNCTION *****************/ static Hor_Edge_Map *es_test ( Hor_Image *img, Hor_UI_Device device, Hor_UI_Info di ) { Hor_Edge_Map *edge_map; Hor_Image *new_img = convert_image ( img, imtype ); /* Coarse feature map parameters */ int cvs, chs, cvo, cho, max_nindices; /* edge detection parameters */ int max_nedges, max_nstrings; Hor_ES_Filter filter; int mask_size, es_nthresh; Hor_Bool logg, scale, impdir, subp; hor_real thresh, lthresh, uthresh, ss; /* display image as background for edges */ hor_ui_image ( di, img, 0, 0, img->w.height, img->w.width ); /* read edge detection parameters */ hor_edge_opt_read ( "test.cfg", "Edge", "", HOR_OPT_ERROR, NULL, NULL, &max_nedges, &max_nstrings, &cvs, &chs, &cvo, &cho, &max_nindices ); /* read edge detection parameters specific to PP's edge detector */ hor_es_opt_read ( "test.cfg", "Edge", "ES", HOR_OPT_ERROR, &filter, &mask_size, &logg, &scale, &thresh, &impdir, &subp, <hresh, &uthresh, &es_nthresh, &ss ); /* create edge map */ edge_map = hor_edge_map_alloc ( new_img->w.height, new_img->w.width, max_nedges, max_nstrings, cvs, chs, cvo, cho, max_nindices ); /* perform edge detection */ hor_es_find_edges ( new_img, NULL, filter, mask_size, logg, scale, thresh, impdir, subp, lthresh, uthresh, es_nthresh, ss, HOR_IMP_LOCAL_TOP_LEFT, edge_map ); /* display edge map */ printf ( "Displaying edges created by Phil Palmer's `es' algorithm\n" ); hor_edge_map_display ( di, edge_map, HOR_IMP_LOCAL_TOP_LEFT, HOR_GREEN, HOR_BLUE, HOR_YELLOW, HOR_RED, HOR_BLUE ); /* use if(0) here instead of #if 0 so that code will be compiled and will avoid "defined but not used" compiler warning */ if(0) /* write edges to file */ hor_edge_map_print ( "cars.edg", edge_map ); /* wait for a key press in the display window */ printf ( "press any key in the window to continue\n" ); hor_imp_event_wait_va ( device, NULL, di, HOR_EDGE_MAP, edge_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, edgel_print, HOR_FALSE, HOR_RED, HOR_GREEN, NULL ); hor_im_free(new_img); return edge_map; } #endif /* #ifdef TEST_ES */ #ifdef TEST_HT /******** PHIL PALMER'S HOUGH TRANSFORM LINE DETECTOR TEST FUNCTION *********/ static Hor_Line_Map *ht_test ( Hor_Edge_Map *edge_map, Hor_Image *img, Hor_UI_Device device, Hor_UI_Info di ) { Hor_Line_Map *line_map; /* Coarse feature map parameters */ int cvs, chs, cvo, cho, max_nindices; /* line detection parameters */ int max_nlines, max_npoints; hor_real proximity, kwidthr, kwidtht, rthresh, rrho, rtheta; Hor_Bool proj, opt; int ht_nthresh, xlength; /* display image as background for lines */ hor_ui_image ( di, img, 0, 0, img->w.height, img->w.width ); /* read line detection parameters */ hor_line_opt_read ( "test.cfg", "Line", "", HOR_OPT_ERROR, NULL, NULL, &max_nlines, &max_npoints, &cvs, &chs, &cvo, &cho, &max_nindices ); /* read Hough transform line detection parameters */ hor_ht_opt_read ( "test.cfg", "Line", "HT", HOR_OPT_ERROR, &proximity, &proj, &kwidthr, &kwidtht, &rthresh, &ht_nthresh, &xlength, &rrho, &rtheta, &opt); /* create line map */ line_map = hor_line_map_alloc ( edge_map->w.height, edge_map->w.width, max_nlines, max_npoints, cvs, chs, cvo, cho, max_nindices ); /* perform Phil Palmer's Hough transform line finder using the edge map as input */ hor_ht_compute_lines ( edge_map, proximity, proj, kwidthr, kwidtht, rthresh, ht_nthresh, xlength, rrho, rtheta, opt, HOR_IMP_LOCAL_CENTRE, line_map ); /* display line map */ printf ( "Displaying lines created by Phil Palmer's `ht' algorithm\n" ); hor_line_map_display ( di, line_map, HOR_IMP_LOCAL_TOP_LEFT, HOR_GREEN, HOR_BLUE, HOR_YELLOW, HOR_BLUE ); /* wait for a key press in the display window */ printf ( "press any key in the window to continue\n" ); hor_imp_event_wait_va ( device, NULL, di, HOR_LINE_MAP, line_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, line_print, HOR_FALSE, HOR_RED, HOR_GREEN, HOR_BLUE, HOR_BLACK, NULL ); return line_map; } #endif /* #ifdef TEST_HT */ #ifdef TEST_JF /************* PHIL PALMER'S JUNCTION DETECTOR TEST FUNCTION **************/ static Hor_Junction_Map *jf_test ( Hor_Line_Map *line_map, Hor_Image *img, Hor_UI_Device device, Hor_UI_Info di ) { Hor_Junction_Map *junction_map; /* Coarse feature map parameters */ int cvs, chs, cvo, cho, max_nindices; /* junction finder parameters */ int max_njunctions; hor_real ax, bx, px, tolr, tolth, mx; Hor_Bool triple; /* line detection parameters */ int max_nlines; /* display image as background for junctions */ hor_ui_image ( di, img, 0, 0, img->w.height, img->w.width ); /* read junction detection parameters */ hor_junction_opt_read ( "test.cfg", "Junction", "", HOR_OPT_ERROR, NULL, NULL, &max_njunctions, &cvs, &chs, &cvo, &cho, &max_nindices ); /* read line detection parameters */ hor_line_opt_read ( "test.cfg", "Line", "", HOR_OPT_ERROR, NULL, NULL, &max_nlines, NULL, NULL, NULL, NULL, NULL, NULL ); /* read Hough transform line detection parameters */ hor_jf_opt_read ( "test.cfg", "Junction", "", HOR_OPT_ERROR, &ax, &bx, &px, &tolr, &tolth, &triple, &mx ); /* create line map */ junction_map = hor_junction_map_alloc ( line_map->w.height, line_map->w.width, max_njunctions, max_nlines, cvs, chs, cvo, cho, max_nindices ); /* find junctions in line map */ hor_jf_find_junctions ( line_map, ax, bx, px, tolr, tolth, triple, mx, HOR_IMP_LOCAL_TOP_LEFT, junction_map ); find_image_center(junction_map, &x_offset, &y_offset); x_offset = -x_offset; y_offset = -y_offset; printf("X offset = %f, Y offset = %f\n", x_offset, y_offset); /* display junctions */ printf ( "Displaying junctions created by Phil Palmer's `jf' algorithm\n" ); hor_junction_map_display ( di, junction_map, HOR_IMP_LOCAL_TOP_LEFT, HOR_GREEN, HOR_PURPLE, HOR_YELLOW, HOR_BLUE ); /* wait for a key press in the display window */ printf ( "press any key in the window to continue\n" ); hor_imp_event_wait_va ( device, NULL, /* arguments for displaying junctions */ di, HOR_JUNCTION_MAP, junction_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, junction_print, HOR_FALSE, HOR_RED, HOR_BLACK, HOR_RED, HOR_BLACK, /* arguments for displaying junction lines */ di, HOR_JUNCLINE_MAP, junction_map, HOR_IMP_LOCAL_TOP_LEFT, NULL, juncline_print, HOR_FALSE, HOR_RED, HOR_BLACK, HOR_RED, HOR_BLACK, NULL ); return junction_map; } #endif /* #ifdef TEST_JF */ int read_worldpoints(int max_points) { int i; FILE *in; if((in = fopen("worldpoints.txt","r")) == NULL) return -1; for(i = 0; i < max_points; i++) { if(fscanf(in, "%lf %lf %lf", &world_points[i].x, &world_points[i].y, &world_points[i].z) != 3) { fclose(in); return i; } } fclose(in); return i; } void find_image_center(Hor_Junction_Map *junction_map, hor_real *x, hor_real *y) { int i; hor_real minx, maxx, miny, maxy, currx, curry; assert(junction_map->njunctions > 1); /* minx = maxx = junction_map->junction[0].pos.x; miny = maxy = junction_map->junction[0].pos.y; for(i = 1; i < junction_map->njunctions; i++) { currx = junction_map->junction[i].pos.x; curry = junction_map->junction[i].pos.y; if(currx < minx) { minx = currx; } else if(currx > maxx) { maxx = currx; } if(curry < miny) { miny = curry; } else if (curry > maxy) { maxy = curry; } } *x = (minx + maxx) / 2.0; *y = (miny + maxy) / 2.0; */ *x = 160; *y = 120; } void print_world_points() { int i; for(i = 0; i < num_world_points; i++) { printf("%d: (%f, %f, %f)\n", i, world_points[i].x, world_points[i].y, world_points[i].z); } } int num_mappings(Hor_Bool print) { int num = 0; int i; for(i = 0; i < MAX_POINTS; i++) { if(mappings[i].mapped == HOR_TRUE) { num++; if(print == HOR_TRUE) printf("Mapping %d: (%f, %f) to (%f, %f, %f)\n", num, mappings[i].image_point.x, mappings[i].image_point.y, world_points[i].x, world_points[i].y, world_points[i].z); } } return num; } /* * threshold_image * Applys a binary thresholding function to the image. * */ int threshold_image(int threshold, Hor_Image *img) { hor_uchar *imgdata; int width, height, x, y, i; assert(img->type == HOR_UCHAR); assert(img->format == HOR_INTENSITY); imgdata = img->data.intensity.uc[0]; width = img->w.width; height = img->w.height; i = 0; printf("Thresholding image (%d x %d)\n", width, height); for(x = 0; x < width; x++) for(y = 0; y < height; y++) { imgdata[i] = imgdata[i] < threshold ? 0 : 255; i++; } return 0; } int calibrate_camera(Hor_UI_Device device, Hor_UI_Info axes_info) { Hor_MatVec *A, *U, *S, *VT, *R, *I; Hor_MatVec *AT, *ATA, *ATAi, *b, *soln; /*** Only used in the last part to determine Tz and fx. ***/ Hor_Mat44 view; Hor_Vec4 axes[6], current_axis; Hor_UI_Font font; int i; int r = 0; int first_point = -1; hor_real v1, v2, v3, v4, v5, v6, v7, v8; hor_real gamma; /*** Our ultimate output. ***/ hor_real r11, r12, r13, r21, r22, r23, r31, r32, r33; hor_real tx, ty, tz; hor_real alpha; hor_real fx; A = hor_mv_alloc(HOR_MATVEC_GEN, num_mapped_points, 8); U = hor_mv_alloc(HOR_MATVEC_GEN, num_mapped_points, 8); S = hor_mv_alloc(HOR_MATRIX_DIAGONAL, 8, 8); VT = hor_mv_alloc(HOR_MATVEC_GEN, 8, 8); I = hor_mvs_ident(3); /*** Construct the A matrix ***/ for(i = 0; i < MAX_POINTS; i++) { if(mappings[i].mapped == HOR_TRUE) { if(first_point == -1) first_point = i; /* Find the first mapped world point -- we will use it to test for sign change later. */ hor_mv_el_set(A, r, 0, mappings[i].image_point.x*world_points[i].x); hor_mv_el_set(A, r, 1, mappings[i].image_point.x*world_points[i].y); hor_mv_el_set(A, r, 2, mappings[i].image_point.x*world_points[i].z); hor_mv_el_set(A, r, 3, mappings[i].image_point.x); hor_mv_el_set(A, r, 4, -mappings[i].image_point.y*world_points[i].x); hor_mv_el_set(A, r, 5, -mappings[i].image_point.y*world_points[i].y); hor_mv_el_set(A, r, 6, -mappings[i].image_point.y*world_points[i].z); hor_mv_el_set(A, r, 7, -mappings[i].image_point.y); r++; } } assert(r == num_mapped_points); hor_mv_svd(A, U, S, VT, NULL); v1 = hor_mv_el_get(VT, 7, 0); v2 = hor_mv_el_get(VT, 7, 1); v3 = hor_mv_el_get(VT, 7, 2); v4 = hor_mv_el_get(VT, 7, 3); v5 = hor_mv_el_get(VT, 7, 4); v6 = hor_mv_el_get(VT, 7, 5); v7 = hor_mv_el_get(VT, 7, 6); v8 = hor_mv_el_get(VT, 7, 7); printf("SVD values are %f, %f, %f, %f, %f, %f, %f, %f.\n", hor_mv_el_get(S, 0,0), hor_mv_el_get(S, 1,1), hor_mv_el_get(S, 2,2), hor_mv_el_get(S, 3,3), hor_mv_el_get(S, 4,4), hor_mv_el_get(S, 5,5), hor_mv_el_get(S, 6,6), hor_mv_el_get(S, 7,7)); printf("V is (%f, %f, %f, %f, %f, %f, %f, %f)\n", v1, v2, v3, v4, v5, v6, v7, v8); hor_mv_free(U); hor_mv_free(S); hor_mv_free(VT); S = hor_mv_alloc(HOR_MATRIX_DIAGONAL, 3, 3); U = hor_mv_alloc(HOR_MATVEC_GEN, 3, 3); VT = hor_mv_alloc(HOR_MATVEC_GEN, 3, 3); gamma = sqrt(v1*v1 + v2*v2 + v3*v3); alpha = sqrt(v5*v5 + v6*v6 + v7*v7) / gamma; r21 = v1 / gamma; r22 = v2 / gamma; r23 = v3 / gamma; r11 = v5 / (gamma*alpha); r12 = v6 / (gamma*alpha); r13 = v7 / (gamma*alpha); r31 = (r12*r23 - r22*r13); r32 = -(r11*r23 - r21*r13); r33 = (r11*r22 - r21*r12); ty = v4 / gamma; tx = v8 / (gamma * alpha); R = hor_mv_alloc(HOR_MATVEC_GEN, 3, 3); hor_mv_el_set(R, 0, 0, r11); hor_mv_el_set(R, 0, 1, r12); hor_mv_el_set(R, 0, 2, r13); hor_mv_el_set(R, 1, 0, r21); hor_mv_el_set(R, 1, 1, r22); hor_mv_el_set(R, 1, 2, r23); hor_mv_el_set(R, 2, 0, r31); hor_mv_el_set(R, 2, 1, r32); hor_mv_el_set(R, 2, 2, r33); printf("R before orthogonalization:\n%f %f %f\n%f %f %f\n%f %f %f\n\n%f %f %f\n", r11, r12, r13, r21, r22, r23, r31, r32, r33, r11*r11+r12*r12+r13*r13, r21*r21+r22*r22+r23*r23, r31*r31+r32*r32+r33*r33); hor_mv_svd(R, U, S, VT, NULL); hor_mvq_prodNN(U, VT, R); r11 = hor_mv_el_get(R, 0, 0); r12 = hor_mv_el_get(R, 0, 1); r13 = hor_mv_el_get(R, 0, 2); r21 = hor_mv_el_get(R, 1, 0); r22 = hor_mv_el_get(R, 1, 1); r23 = hor_mv_el_get(R, 1, 2); r31 = hor_mv_el_get(R, 2, 0); r32 = hor_mv_el_get(R, 2, 1); r33 = hor_mv_el_get(R, 2, 2); printf("R after orthogonalization:\n%f %f %f\n%f %f %f\n%f %f %f\n\n%f %f %f\n", r11, r12, r13, r21, r22, r23, r31, r32, r33, r11*r11+r12*r12+r13*r13, r21*r21+r22*r22+r23*r23, r31*r31+r32*r32+r33*r33); /* * Now, we have R, the first two components of T, gamma, and alpha. We * still need to determine the sign of gamma, however, and if necessary, * switch signs of the first two components of T, and the first two rows * of R. */ if((mappings[first_point].image_point.x - x_offset)* (r11*world_points[first_point].x + r12*world_points[first_point].y + r13*world_points[first_point].z + tx) > 0) { /* Reverse the signs */ printf("Reversing signs of first two rows of R, Tx, and Ty.\n"); r11 = -r11; r12 = -r12; r13 = -r13; r21 = -r21; r22 = -r22; r23 = -r23; hor_mv_el_set(R, 0, 0, r11); hor_mv_el_set(R, 0, 1, r12); hor_mv_el_set(R, 0, 2, r13); hor_mv_el_set(R, 1, 0, r21); hor_mv_el_set(R, 1, 1, r22); hor_mv_el_set(R, 1, 2, r23); tx = -tx; ty = -ty; } printf("Alpha is %f\n",alpha); printf("Tx is (%f, %f, ???)\n", tx, ty); /* Finally, using each of the points mapped, solve the overconstrained * system of n linear equations (6.14) in Trucco and Verri to obtain Tz * and fx. */ hor_mv_free(A); A = hor_mv_alloc(HOR_MATVEC_GEN, num_mapped_points, 2); b = hor_mv_alloc(HOR_MATVEC_GEN, num_mapped_points, 1); /*** Construct A and b. ***/ r = 0; for(i = 0; i < MAX_POINTS; i++) { if(mappings[i].mapped == HOR_TRUE) { hor_mv_el_set(A, r, 0, mappings[i].image_point.x); hor_mv_el_set(A, r, 1, (r11*world_points[i].x + r12*world_points[i].y + r13*world_points[i].z + tx)); hor_mv_el_set(b, r, 0, -mappings[i].image_point.x*(r31*world_points[i].x + r32*world_points[i].y + r33*world_points[i].z)); r++; } } AT = hor_mvs_tr(A); ATA = hor_mvs_prodNN(AT, A); ATAi = hor_mvs_inv(ATA); soln = hor_mvs_prod3p(ATAi, HOR_FALSE, HOR_FALSE, AT, HOR_FALSE, HOR_FALSE, b, HOR_FALSE, HOR_FALSE); tz = hor_mv_el_get(soln, 0, 0); fx = hor_mv_el_get(soln, 1, 0); printf("***** FINAL OUTPUT *****\n"); printf("R:\n%f %f %f\n%f %f %f\n%f %f %f\n\n", r11, r12, r13, r21, r22, r23, r31, r32, r33); printf("(Tx, Ty, Tz) = %f %f %f\n\n", tx, ty, tz); printf("Alpha = %f\n\n", alpha); printf("fx = %f\n\n", fx); hor_ui_set_coord_sys(axes_info, HOR_UI_COORD_YUP); hor_ui_set_ortho(axes_info, -1.0, 1.0, 0, 199, -1.0, 1.0, 0, 199); font = hor_ui_load_font(axes_info, "-*-application-bold-r-*-*-10-*-*-*-*-*-iso8859-*"); /*** Transform our axes. ***/ hor_m44q_fill(&view, K, 0, -K, 0, -K*K, K, -K*K, 0, K*K, K, K*K, 0, 0, 0, 0, 1.0); hor_v4q_fill(&axes[0], 1.0, 0.0, 0.0, 1.0); hor_v4q_fill(&axes[1], 0.0, 1.0, 0.0, 1.0); hor_v4q_fill(&axes[2], 0.0, 0.0, 1.0, 1.0); hor_v4q_fill(&axes[3], r11, r21, r31, 1.0); hor_v4q_fill(&axes[4], r12, r22, r32, 1.0); hor_v4q_fill(&axes[5], r13, r23, r33, 1.0); /*** Draw the coordinate axes. ***/ hor_m44v4q_prod(&view, &axes[0], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_WHITE); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "X", font, HOR_WHITE, HOR_UI_CENTERED); hor_m44v4q_prod(&view, &axes[1], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_WHITE); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "Y", font, HOR_WHITE, HOR_UI_CENTERED); hor_m44v4q_prod(&view, &axes[2], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_WHITE); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "Z", font, HOR_WHITE, HOR_UI_CENTERED); hor_m44v4q_prod(&view, &axes[3], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_RED); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "X'", font, HOR_RED, HOR_UI_CENTERED); hor_m44v4q_prod(&view, &axes[4], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_GREEN); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "Y'", font, HOR_GREEN, HOR_UI_CENTERED); hor_m44v4q_prod(&view, &axes[5], ¤t_axis); hor_ui_line(axes_info, 0.0, 0.0, current_axis.y/current_axis.u, current_axis.x/current_axis.u, HOR_BLUE); hor_ui_text(axes_info, current_axis.y/current_axis.u, current_axis.x/current_axis.u, "Z'", font, HOR_BLUE, HOR_UI_CENTERED); printf("press any key in the window or click to continue\n"); hor_ui_wait(axes_info); return r; } int main(int argc, char * argv[]) { Hor_UI_Device device; Hor_UI_Info di, axes_info; Hor_Image *img; char *filename; int i; #if defined(TEST_CANNY) || defined(TEST_ES) Hor_Edge_Map *edge_map = NULL; #endif #if defined(TEST_ORTHOG) || defined(TEST_HT) Hor_Line_Map *line_map = NULL; #endif #ifdef TEST_JF Hor_Junction_Map *junction_map = NULL; #endif device = hor_ui_init("X11"); /* IM READ TEST */ #ifdef SIMULATE_IMAGE img = square(); #else if(argc < 2) { filename = "cars.pgm"; } else { filename = argv[1]; } img = hor_im_read ( filename, HOR_PGM, HOR_UCHAR, HOR_INTENSITY, 0 ); if ( img == NULL ) { char s[300]; sprintf ( s, "could not open image file \"%s\"", filename ); perror ( s ); exit(-1); } #endif if((num_world_points = read_worldpoints(MAX_POINTS)) < 1) { fprintf(stderr, "ERROR: Couldn't read file of world coordinates.\n"); exit(1); } printf("Read %d world points.\n", num_world_points); for(i = 0; i < num_world_points; i++) { mappings[i].mapped = HOR_FALSE; } threshold_image(100, img); /* UI TEST */ #if 0 /* use this to blow up for small images like the default cars image */ di = hor_ui_new ( device, HOR_WINDOW_GRAYSCALE, 0, 0, 2*img->w.height, 2*img->w.width, filename, NULL ); hor_ui_set_ortho ( di, 0.0, (double) img->w.height, 0, 2*img->w.height, 0.0, (double) img->w.width, 0, 2*img->w.width ); #else /* use this to display bigger images at normal size */ di = hor_ui_new ( device, HOR_WINDOW_GRAYSCALE, 0, 0, img->w.height, img->w.width, filename, NULL ); axes_info = hor_ui_new(device, HOR_WINDOW_COLOR, 0, 0, 200, 200, "Rotation matrix", NULL); #endif #if defined(TEST_CANNY) /* Canny edge detector test */ edge_map = canny_test ( img, device, di ); #elif defined(TEST_ES) /* Phil Palmer's "es" edge detector test */ edge_map = es_test ( img, device, di ); #endif #if defined(TEST_ORTHOG) /* Line fitting test */ line_map = orthog_test ( edge_map, img, device, di ); #elif defined(TEST_HT) /* Line fitting test */ line_map = ht_test ( edge_map, img, device, di ); #endif #ifdef TEST_JF /* Junction fitting test */ junction_map = jf_test ( line_map, img, device, di ); #endif num_mapped_points = num_mappings(HOR_TRUE); calibrate_camera(device, axes_info); #ifdef TEST_JF /* free junction map */ hor_junction_map_free ( junction_map ); #endif #if defined(TEST_ORTHOG) || defined(TEST_HT) /* free line map */ hor_line_map_free ( line_map ); #endif #if defined(TEST_CANNY) || defined(TEST_ES) /* free edge map */ hor_edge_map_free ( edge_map ); #endif printf("\n"); hor_ui_delete(di); hor_ui_delete(axes_info); hor_ui_uninit(device); hor_im_free(img); return 0; }