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