int main(void) { koki_camera_params_t params; params.size.x = WIDTH; params.size.y = HEIGHT; params.principal_point.x = params.size.x / 2; params.principal_point.y = params.size.y / 2; params.focal_length.x = 571.0; params.focal_length.y = 571.0; int fd = koki_v4l_open_cam("/dev/video0"); struct v4l2_format fmt = koki_v4l_create_YUYV_format(WIDTH, HEIGHT); koki_v4l_set_format(fd, fmt); int num_buffers = 1; koki_buffer_t *buffers; buffers = koki_v4l_prepare_buffers(fd, &num_buffers); koki_v4l_start_stream(fd); while (1){ uint8_t *yuyv = koki_v4l_get_frame_array(fd, buffers); IplImage *frame = koki_v4l_YUYV_frame_to_RGB_image(yuyv, WIDTH, HEIGHT); IplImage *thresholded; thresholded = koki_threshold_adaptive(frame, 5, 3, KOKI_ADAPTIVE_MEAN); cvShowImage("thresh", thresholded); koki_labelled_image_t *l = koki_label_image(thresholded, 128); for (int i=0; i<l->clips->len; i++){ if (!koki_label_useable(l, i)) continue; GSList *contour = koki_contour_find(l, i); koki_quad_t *quad = koki_quad_find_vertices(contour); if (quad == NULL){ koki_contour_free(contour); continue; } koki_contour_draw(frame, contour); koki_quad_refine_vertices(quad); koki_quad_draw(frame, quad); koki_marker_t *marker; marker = koki_marker_new(quad); if (koki_marker_recover_code(marker, frame)){ koki_pose_estimate(marker, 0.11, ¶ms); koki_bearing_estimate(marker); printf("marker code: %d\n", marker->code); } koki_contour_free(contour); koki_quad_free(quad); koki_marker_free(marker); }//for cvShowImage("frame", frame); cvWaitKey(1); koki_labelled_image_free(l); cvReleaseImage(&thresholded); cvReleaseImage(&frame); } return 0; cvDestroyWindow("frame"); cvDestroyWindow("thresh"); }
void debug(IplImage *frame) { cvNamedWindow("thresholded", CV_WINDOW_AUTOSIZE); cvNamedWindow("output", CV_WINDOW_AUTOSIZE); IplImage *thresholded = koki_threshold_adaptive(frame, 5, 5, KOKI_ADAPTIVE_MEAN); assert(thresholded != NULL); cvShowImage("thresholded", thresholded); //cvWaitKey(0); koki_labelled_image_t *l = koki_label_image(frame, 128); int waited = 0; for (int i=0; i<l->clips->len; i++){ if (!koki_label_useable(l, i)) continue; printf("=====================================\nlabel %d\n", i); GSList *contour = koki_contour_find(l, i); koki_contour_draw(frame, contour); cvShowImage("output", frame); cvWaitKey(WAIT); waited = 1; koki_quad_t *quad = koki_quad_find_vertices(contour); if (quad == NULL){ printf("Quad not found\n"); koki_contour_free(contour); continue; } koki_quad_refine_vertices(quad); koki_quad_draw(frame, quad); koki_marker_t *marker; marker = koki_marker_new(quad); assert(marker != NULL); if (koki_marker_recover_code(marker, frame)){ printf("Marker found. Code: %d, Z rotation: %f\n", marker->code, marker->rotation.z); } else { printf("Either not a marker, or failed to recover code\n"); } koki_marker_free(marker); koki_quad_free(quad); koki_contour_free(contour); cvShowImage("output", frame); cvWaitKey(WAIT); waited = 1; }//for if (!waited) cvWaitKey(WAIT); koki_labelled_image_free(l); cvDestroyWindow("thresholded"); cvDestroyWindow("output"); }
/** * @brief a higher-level function that does everything necessary to return * an array of markers that thare in the given frame, with a user- * specified function for determining the marker width based on the * code * * The function pointer, \c fp, should point to a function that takes as * argument a marker code, and returns a float representing the width of * a marker with said code. * * @param frame the input image * @param fp the function pointer * @param params the camera params for the camera at \c frame's resolution * @return a \c GptrArray* containing all of the found markers */ GPtrArray* koki_find_markers_fp(IplImage *frame, float (*fp)(int), koki_camera_params_t *params) { koki_labelled_image_t *labelled_image; GSList *contour; koki_quad_t *quad; koki_marker_t *marker; GPtrArray *markers = NULL; assert(frame != NULL); /* labelling */ labelled_image = koki_label_adaptive( frame, 11, 5 ); if (labelled_image == NULL) return NULL; /* init markers array */ markers = g_ptr_array_new(); /* loop though all regions */ for (uint16_t i=0; i<labelled_image->clips->len; i++){ /* make sure it's big enough, etc... */ if (!koki_label_useable(labelled_image, i)) continue; /* get contour */ contour = koki_contour_find(labelled_image, i); /* find vertices */ quad = koki_quad_find_vertices(contour); if (quad == NULL){ koki_contour_free(contour); continue; } /* refine vertices */ koki_quad_refine_vertices(quad); /* create a base marker */ marker = koki_marker_new(quad); assert(marker != NULL); /* recover code */ if (koki_marker_recover_code(marker, frame)){ assert(marker != NULL); koki_pose_estimate(marker, fp(marker->code), params); koki_rotation_estimate(marker); koki_bearing_estimate(marker); /* append the marker to the output array */ g_ptr_array_add(markers, marker); } else { /* not a useful marker, free it */ koki_marker_free(marker); } /* cleanup */ koki_contour_free(contour); koki_quad_free(quad); }//for /* clean up */ koki_labelled_image_free(labelled_image); return markers; }