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");
}
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");
}
