static void update_view(vx_gtk_buffer_manager_t * gtk)
{
// This order of these two mutex locks should prevent deadloc
// even if a user sends op codes while the user holds the GDK mutex
gdk_threads_enter();
pthread_mutex_lock(>k->mutex);
// Clear XXX Double buffered?
GList * children = gtk_container_get_children(GTK_CONTAINER(gtk->window));
for(GList * iter = children; iter != NULL; iter = g_list_next(iter))
gtk_widget_destroy(GTK_WIDGET(iter->data));
g_list_free(children);
// Rebuild from scratch
GtkWidget * box = gtk_vbox_new(0, 10);
GtkWidget * widget = NULL;
zarray_t *layers = zhash_values(gtk->layers); // contents: layer_info_t*
zarray_sort(layers, layer_info_compare);
for (int lidx = 0; lidx < zarray_size(layers); lidx++) {
layer_info_t *linfo = NULL;
zarray_get(layers, lidx, &linfo);
// Draw the layer name:
widget = gtk_label_new("");
char * text = sprintf_alloc("<b>Layer %d</b>", linfo->layer_id);
gtk_label_set_markup(GTK_LABEL(widget), text);
free(text);
//gtk_container_add(GTK_CONTAINER(box), widget);
gtk_box_pack_start(GTK_BOX(box), widget, FALSE, FALSE, 0);
// Make a checkbox for each buffer
zarray_t *buffers = zhash_values(linfo->buffers); // contents: buffer_info_t*
zarray_sort(buffers, buffer_info_compare);
for (int i = 0; i < zarray_size(buffers); i++) {
buffer_info_t * buffer = NULL;
zarray_get(buffers, i, &buffer);
assert(buffer != NULL);
widget = gtk_check_button_new_with_label(buffer->name);
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(widget), buffer->enabled);
g_signal_connect (G_OBJECT(widget), "toggled",
G_CALLBACK (buffer_checkbox_changed), buffer);
//gtk_container_add(GTK_CONTAINER(box), widget);
gtk_box_pack_start(GTK_BOX(box), widget, FALSE, FALSE, 0);
}
zarray_destroy(buffers);
}
gtk_container_add(GTK_CONTAINER(gtk->window), box);
gtk_widget_show_all(box);
zarray_destroy(layers);
pthread_mutex_unlock(>k->mutex);
gdk_threads_leave();
}
// return 1 if the quad looks okay, 0 if it should be discarded
int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct quad *quad)
{
int res = 0;
int sz = zarray_size(cluster);
if (sz < 4) // can't fit a quad to less than 4 points
return 0;
/////////////////////////////////////////////////////////////
// Step 1. Sort points so they wrap around the center of the
// quad. We will constrain our quad fit to simply partition this
// ordered set into 4 groups.
// compute a bounding box so that we can order the points
// according to their angle WRT the center.
int xmax = 0, xmin = 9999999, ymax = 0, ymin = 9999999;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
xmax = imax(xmax, p->x);
xmin = imin(xmin, p->x);
ymax = imax(ymax, p->y);
ymin = imin(ymin, p->y);
}
int cx = (xmin + xmax) / 2;
int cy = (ymin + ymax) / 2;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
p->theta = atan2f(p->y - cy, p->x - cx);
}
zarray_sort(cluster, pt_compare_theta);
// remove duplicate points. (A byproduct of our segmentation system.)
if (1) {
int outpos = 1;
struct pt *last;
zarray_get_volatile(cluster, 0, &last);
for (int i = 1; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (p->x != last->x || p->y != last->y) {
if (i != outpos) {
struct pt *out;
zarray_get_volatile(cluster, outpos, &out);
memcpy(out, p, sizeof(struct pt));
}
outpos++;
}
last = p;
}
cluster->size = outpos;
sz = outpos;
}
if (sz < 4)
return 0;
/////////////////////////////////////////////////////////////
// Step 2. Precompute statistics that allow line fit queries to be
// efficiently computed for any contiguous range of indices.
struct line_fit_pt *lfps = (struct line_fit_pt *)calloc(sz, sizeof(struct line_fit_pt));
for (int i = 0; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (i > 0) {
memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt));
}
double W = 1;
if (p->x > 0 && p->x+1 < im->width && p->y > 0 && p->y+1 < im->height) {
int grad_x = im->buf[p->y * im->stride + p->x + 1] -
im->buf[p->y * im->stride + p->x - 1];
int grad_y = im->buf[(p->y+1) * im->stride + p->x] -
im->buf[(p->y-1) * im->stride + p->x];
W = sqrtf(grad_x*grad_x + grad_y*grad_y) + 1;
}
lfps[i].Mx += W * p->x;
lfps[i].My += W * p->y;
lfps[i].Mxx += W * p->x * p->x;
lfps[i].Mxy += W * p->x * p->y;
lfps[i].Myy += W * p->y * p->y;
lfps[i].W += W;
}
int indices[4];
if (1) {
if (!quad_segment_maxima(td, cluster, lfps, indices))
goto finish;
} else {
if (!quad_segment_agg(td, cluster, lfps, indices))
goto finish;
}
// printf("%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
if (0) {
// no refitting here; just use those points as the vertices.
// Note, this is useful for debugging, but pretty bad in
// practice since this code path also omits several
// plausibility checks that save us tons of time in quad
// decoding.
for (int i = 0; i < 4; i++) {
struct pt *p;
zarray_get_volatile(cluster, indices[i], &p);
quad->p[i][0] = p->x;
quad->p[i][1] = p->y;
}
res = 1;
} else {
double lines[4][4];
for (int i = 0; i < 4; i++) {
int i0 = indices[i];
int i1 = indices[(i+1)&3];
if (0) {
// if there are enough points, skip the points near the corners
// (because those tend not to be very good.)
if (i1-i0 > 8) {
int t = (i1-i0)/6;
if (t < 0)
t = -t;
i0 = (i0 + t) % sz;
i1 = (i1 + sz - t) % sz;
}
}
double err;
fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
// XXX VALUE?
// printf("%f %d\n", err, i1-i0);
if (err > td->qtp.max_line_fit_mse) {
res = 0;
goto finish;
}
}
for (int i = 0; i < 4; i++) {
// solve for the intersection of lines (i) and (i+1)&3.
// p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1
// are the line directions.
//
// lambda0*u0 - lambda1*u1 = (p1 - p0)
//
// rearrange (solve for lambdas)
//
// [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ]
// [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ]
//
// remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector.
// We want the unit vector, so we need the perpendiculars. Thus, below
// we have swapped the x and y components and flipped the y components.
double A00 = lines[i][3], A01 = -lines[(i+1)&3][3];
double A10 = -lines[i][2], A11 = lines[(i+1)&3][2];
double B0 = -lines[i][0] + lines[(i+1)&3][0];
double B1 = -lines[i][1] + lines[(i+1)&3][1];
double det = A00 * A11 - A10 * A01;
// inverse.
double W00 = A11 / det, W01 = -A01 / det;
if (fabs(det) < 0.001) {
res = 0;
goto finish;
}
// solve
double L0 = W00*B0 + W01*B1;
// compute intersection
quad->p[i][0] = lines[i][0] + L0*A00;
quad->p[i][1] = lines[i][1] + L0*A10;
if (0) {
// we should get the same intersection starting
// from point p1 and moving L1*u1.
double W10 = -A10 / det, W11 = A00 / det;
double L1 = W10*B0 + W11*B1;
double x = lines[(i+1)&3][0] - L1*A10;
double y = lines[(i+1)&3][1] - L1*A11;
assert(fabs(x - quad->p[i][0]) < 0.001 &&
fabs(y - quad->p[i][1]) < 0.001);
}
res = 1;
}
}
// reject quads with edges that are too short or long.
if (1) {
for (int i = 0; i < 3; i++) {
double dist2 = sq(quad->p[i][0] - quad->p[i+1][0]) +
sq(quad->p[i][1] - quad->p[i+1][1]);
if (dist2 < sq(6) || dist2 > sq(4096)) {
res = 0;
goto finish;
}
}
}
// reject quads whose cumulative angle change isn't equal to 2PI
if (1) {
double total = 0;
for (int i = 0; i < 4; i++) {
int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3;
double theta0 = atan2f(quad->p[i0][1] - quad->p[i1][1],
quad->p[i0][0] - quad->p[i1][0]);
double theta1 = atan2f(quad->p[i2][1] - quad->p[i1][1],
quad->p[i2][0] - quad->p[i1][0]);
double dtheta = theta0 - theta1;
if (dtheta < 0)
dtheta += 2*M_PI;
if (dtheta < td->qtp.critical_rad || dtheta > (M_PI - td->qtp.critical_rad))
res = 0;
total += dtheta;
}
if (total < 6.2 || total > 6.4) {
res = 0;
goto finish;
}
}
// adjust pixel coordinates; all math up 'til now uses pixel
// coordinates in which (0,0) is the lower left corner. But each
// pixel actually spans from to [x, x+1), [y, y+1) the mean value of which
// is +.5 higher than x & y.
for (int i = 0; i < 4; i++) {
quad->p[i][0] += .5;
quad->p[i][1] += .5;
}
finish:
/*
if (res) {
static image_u8_t *im;
if (im == NULL)
im = image_u8_create(1920,1080);
for (int j = 0; j < 4; j++) {
int i0 = indices[j];
int i1 = indices[(j+1)&3];
if (i1 < i0)
i1 += sz;
for (int i = i0; i <= i1; i++) {
struct pt *p;
zarray_get_volatile(cluster, i % sz, &p);
im->buf[((int) p->y)*im->stride + ((int) p->x)] = 64 + 128*(j%2);
}
}
char name[1024];
sprintf(name, "debug_seg.pnm", utime_now());
image_u8_write_pnm(im, name);
}
*/
free(lfps);
return res;
}
// this loop tries to run at X fps, and issue render commands
static void * render_loop(void * foo)
{
state_t * state = foo;
if (verbose)printf("Starting render thread!\n");
uint64_t render_count = 0;
uint64_t last_mtime = vx_mtime();
double avgDT = 1.0f/state->target_frame_rate;
uint64_t avg_loop_us = 3000; // initial render time guess
while (state->rendering) {
int64_t sleeptime = (1000000 / state->target_frame_rate) - (int64_t) avg_loop_us;
if (sleeptime > 0)
usleep(sleeptime); // XXX fix to include render time
// Diagnostic tracking
uint64_t mtime_start = vx_mtime(); // XXX
avgDT = avgDT*.9 + .1 * (mtime_start - last_mtime)/1000;
last_mtime = mtime_start;
render_count++;
if (verbose) {
if (render_count % 100 == 0)
printf("Average render DT = %.3f FPS = %.3f avgloopus %"PRIu64" sleeptime = %"PRIi64"\n",
avgDT, 1.0/avgDT, avg_loop_us, sleeptime);
}
// prep the render data
render_buffer_t rbuf;
rbuf.state = state;
rbuf.width = gtku_image_pane_get_width(state->imagePane);
rbuf.height = gtku_image_pane_get_height(state->imagePane);
if (rbuf.width == 0 && rbuf.height == 0)
continue; // if the viewport is 0,0
// smartly reuse, or reallocate the output pixel buffer when resizing occurs
GdkPixbuf * pixbuf = state->pixbufs[state->cur_pb_idx];
if (pixbuf == NULL || gdk_pixbuf_get_width(pixbuf) != rbuf.width || gdk_pixbuf_get_height(pixbuf) != rbuf.height) {
if (pixbuf != NULL) {
g_object_unref(pixbuf);
free(state->pixdatas[state->cur_pb_idx]);
}
state->pixdatas[state->cur_pb_idx] = malloc(rbuf.width*rbuf.height*3); // can't stack allocate, can be too big (retina)
pixbuf = gdk_pixbuf_new_from_data(state->pixdatas[state->cur_pb_idx], GDK_COLORSPACE_RGB, FALSE, 8,
rbuf.width, rbuf.height, rbuf.width*3, NULL, NULL); // no destructor fn for pix data, handle manually
state->pixbufs[state->cur_pb_idx] = pixbuf;
}
// second half of init:
rbuf.out_buf = gdk_pixbuf_get_pixels(pixbuf);
rbuf.format = GL_RGB;
rbuf.rendered = 0;
// 1 compute all the viewports
render_info_t * rinfo = render_info_create();
rinfo->viewport[0] = rinfo->viewport[1] = 0;
rinfo->viewport[2] = rbuf.width;
rinfo->viewport[3] = rbuf.height;
{
zhash_iterator_t itr;
uint32_t layer_id = 0;
layer_info_t * linfo = NULL;
zhash_iterator_init(state->layer_info_map, &itr);
while(zhash_iterator_next(&itr, &layer_id, &linfo)){
zarray_add(rinfo->layers, &linfo);
}
zarray_sort(rinfo->layers, zvx_layer_info_compare);
}
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t *mm = matd_create(4,4); zarray_add(fp, &mm);
matd_t *pm = matd_create(4,4); zarray_add(fp, &pm);
pthread_mutex_lock(&state->mutex);
for (int i = 0; i < zarray_size(rinfo->layers); i++) {
layer_info_t *linfo = NULL;
zarray_get(rinfo->layers, i, &linfo);
int * viewport = vx_viewport_mgr_get_pos(linfo->vp_mgr, rinfo->viewport, mtime_start);
vx_camera_pos_t *pos = default_cam_mgr_get_cam_pos(linfo->cam_mgr, viewport, mtime_start);
// store viewport, pos
zhash_put(rinfo->layer_positions, &linfo->layer_id, &viewport, NULL, NULL);
zhash_put(rinfo->camera_positions, &linfo->layer_id, &pos, NULL, NULL);
// feed the actual camera/projection matrix to the gl side
vx_camera_pos_model_matrix(pos,mm->data);
vx_camera_pos_projection_matrix(pos, pm->data);
matd_t * pmmm = matd_multiply(pm,mm); zarray_add(fp, &pmmm);
float pm16[16];
vx_util_copy_floats(pmmm->data, pm16, 16);
float eye3[16];
vx_util_copy_floats(pos->eye, eye3, 3);
vx_gl_renderer_set_layer_render_details(state->glrend, linfo->layer_id, viewport, pm16, eye3);
}
// 2 Render the data
task_thread_schedule_blocking(gl_thread, render_task, &rbuf);
render_info_t * old = state->last_render_info;
state->last_render_info = rinfo;
pthread_mutex_unlock(&state->mutex);
// 3 if a render occurred, then swap gtk buffers
if (rbuf.rendered) {
// point to the correct buffer for the next render:
state->cur_pb_idx = (state->cur_pb_idx +1 ) % 2;
// flip y coordinate in place:
vx_util_flipy(rbuf.width*3, rbuf.height, rbuf.out_buf);
// swap the image's backing buffer
g_object_ref(pixbuf); // XXX Since gtku always unrefs with each of these calls, increment accordingly
gtku_image_pane_set_buffer(state->imagePane, pixbuf);
}
// 3.1 If a movie is in progress, also need to serialize the frame
pthread_mutex_lock(&state->movie_mutex);
if (state->movie_file != NULL) {
int last_idx = (state->cur_pb_idx + 1) % 2;
GdkPixbuf * pb = state->pixbufs[last_idx];
movie_frame_t * movie_img = calloc(1, sizeof(movie_frame_t));
movie_img->mtime = mtime_start;
movie_img->width = gdk_pixbuf_get_width(pb);
movie_img->height = gdk_pixbuf_get_height(pb);
movie_img->stride = 3*movie_img->width;
movie_img->buf = malloc(movie_img->stride*movie_img->height);
memcpy(movie_img->buf, state->pixdatas[last_idx], movie_img->stride*movie_img->height);
// Alloc in this thread, dealloc in movie thread
zarray_add(state->movie_pending, & movie_img);
pthread_cond_signal(&state->movie_cond);
}
pthread_mutex_unlock(&state->movie_mutex);
// cleanup
if (old)
render_info_destroy(old);
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
uint64_t mtime_end = vx_mtime();
avg_loop_us = (uint64_t)(.5*avg_loop_us + .5 * 1000 * (mtime_end - mtime_start));
}
if (verbose) printf("Render thread exiting\n");
pthread_exit(NULL);
}
// returns the 35 points associated to the test chart in [x1,y1,x2,y2]
// format if there are more than 35 points will return NULL
matd_t* build_homography(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
frame_t frame = {{0,0}, {im->width-1, im->height-1},
{0,0}, {1,1}};
int good_size = 0;
zarray_t* blobs = zarray_create(sizeof(node_t));
hsv_find_balls_blob_detector(im, frame, met, blobs, buf);
// remove unqualified blobs
if(met.qualify) {
for(int i = 0; i < zarray_size(blobs); i++) {
node_t n;
zarray_get(blobs, i, &n);
if(!blob_qualifies(im, &n, met, buf))
zarray_remove_index(blobs, i, 0);
}
}
if(zarray_size(blobs) == NUM_TARGETS ||zarray_size(blobs) == NUM_CHART_BLOBS) good_size = 1;
zarray_sort(blobs, compare);
int pix_array[zarray_size(blobs)*2];
// iterate through
int idx = 0;
double size = 2.0;
for(int i = 0; i < zarray_size(blobs); i++) {
node_t n;
zarray_get(blobs, i, &n);
loc_t center = { .x = n.ave_loc.x/n.num_children,
.y = n.ave_loc.y/n.num_children};
loc_t parent = { .x = n.id % im->stride,
.y = n.id / im->stride};
if(buf != NULL) {
add_circle_to_buffer(buf, size, center, vx_maroon);
// add_circle_to_buffer(buf, size, parent, vx_olive);
// add_sides_to_buffer(im, buf, 1.0, &n, vx_orange, met);
loc_t* lp = fit_lines(im, &n, buf, met, NULL);
if(lp != NULL) {
// printf("(%d, %d) (%d, %d) (%d, %d) (%d, %d) \n",
// lp[0].x, lp[0].y, lp[1].x, lp[1].y, lp[2].x, lp[2].y, lp[3].x, lp[3].y);
loc_t intersect = get_line_intersection(lp[0], lp[1], lp[2], lp[3]);
if(in_range(im, intersect.x, intersect.y)) {
loc_t ext_lines[2];
extend_lines_to_edge_of_image(im, intersect, center, ext_lines);
add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_blue);
}
for(int i = 0; i < 4; i++) {
pix_array[i*2] = lp[i].x;
pix_array[i*2+1] = lp[i].y;
add_circle_to_buffer(buf, 3.0, lp[i], vx_orange);
}
}
free(n.sides);
// loc_t corners[4] = {{n.box.right, n.box.top},
// {n.box.right, n.box.bottom},
// {n.box.left, n.box.bottom},
// {n.box.left, n.box.top}};
// print extremes of box
// if(1) {
// add_circle_to_buffer(buf, size, corners[0], vx_green);
// add_circle_to_buffer(buf, size, corners[1], vx_yellow);
// add_circle_to_buffer(buf, size, corners[2], vx_red);
// add_circle_to_buffer(buf, size, corners[3], vx_blue);
// for(int j = 0; j < 4; j++) {
// // add_circle_to_buffer(buf, size, corners[j], vx_maroon);
// }
// }
}
}
matd_t* H;
H = dist_homography(pix_array, NUM_TARGETS);
// if(0) {//zarray_size(blobs) == NUM_CHART_BLOBS){
// H = dist_homography(pix_array, NUM_CHART_BLOBS);
// }
// else if(zarray_size(blobs) == NUM_TARGETS){
// H = dist_homography(pix_array, NUM_TARGETS);
// if(met.add_lines) connect_lines(blobs, buf);
// }
// else {
// if(met.dothis)
// printf("num figures: %d\n", zarray_size(blobs));
// return(NULL);
// }
// make projected points
// project_measurements_through_homography(H, buf, blobs, zarray_size(blobs));
zarray_destroy(blobs);
return(H);
}
/*
{ R00, R01, R02, TX,
R10, R11, R12, TY,
R20, R21, R22, TZ,
0, 0, 0, 1 });
*/
double get_rotation(const char* axis, matd_t* H)
{
double cosine, sine, theta;
if(strncmp(axis,"x", 1)) {
cosine = MATD_EL(H, 1, 1);
sine = MATD_EL(H, 2, 1);
}
else if(strncmp(axis,"y", 1)) {
cosine = MATD_EL(H, 0, 0);
sine = MATD_EL(H, 0, 2);
}
else if(strncmp(axis,"z", 1)) {
cosine = MATD_EL(H, 0, 0);
sine = MATD_EL(H, 1, 0);
}
else assert(0);
theta = atan2(sine, cosine);
return(theta);
}
// if buf is NULL, will not fill with points of the homography
void take_measurements(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
// form homography
matd_t* H = build_homography(im, buf, met);
if(H == NULL) return;
// get model view from homography
matd_t* Model = homography_to_pose(H, 654, 655, 334, 224);
// printf("\n");
// matd_print(H, matrix_format);
// printf("\n\n");
// printf("model:\n");
// matd_print(Model, "%15f");
// printf("\n\n");
// matd_print(matd_op("M^-1",Model), matrix_format);
// printf("\n");
// extrapolate metrics from model view
double TX = MATD_EL(Model, 0, 3);
double TY = MATD_EL(Model, 1, 3);
double TZ = MATD_EL(Model, 2, 3);
// double rot_x = get_rotation("x", H);
// double rot_y = get_rotation("y", H);
// double rot_z = get_rotation("z", H);
double cosine = MATD_EL(Model, 0, 0);
double rot_z = acos(cosine) * 180/1.5 - 180;
cosine = MATD_EL(Model, 2, 2);
double rot_x = asin(cosine) * 90/1.3 + 90;
cosine = MATD_EL(Model, 1, 1);
double rot_y = asin(cosine);
char str[200];
sprintf(str, "<<#00ffff,serif-30>> DIST:%lf Offset:(%lf, %lf)\n rot: (%lf, %lf, %lf)\n",
TZ, TX, TY, rot_x, rot_y, rot_z);
vx_object_t *text = vxo_text_create(VXO_TEXT_ANCHOR_BOTTOM_LEFT, str);
vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, text));
// printf("dist: %lf cos:%lf angle: %lf\n", TZ, cosine, theta);
}
