void add_circle_to_buffer(vx_buffer_t* buf, double size, loc_t loc, const float* color)
{
vx_buffer_add_back(buf,
vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_chain(vxo_mat_translate3(loc.x, loc.y, 0),
vxo_mat_scale(size),
vxo_circle(vxo_mesh_style(color)))));
}
void project_measurements_through_homography(matd_t* H, vx_buffer_t* buf,
zarray_t* pix_found, int size)
{
int npoints = NUM_CHART_BLOBS * 2; // line per chart blob
float points[npoints*3];
float* real_world_coords;
if(size == NUM_TARGETS) real_world_coords = target_coords;
else if(size == NUM_CHART_BLOBS) real_world_coords = chart_coords;
else assert(0);
for(int i = 0; i < size; i++) {
// run each real world point through homography and add to buf
double tmp[3] = {real_world_coords[i*2], real_world_coords[i*2+1], 1};
matd_t* xy_matrix = matd_create_data(3,1,tmp);
matd_t* pix_estimated = matd_op("(M)*M",H, xy_matrix);
MATD_EL(pix_estimated,0,0) /= MATD_EL(pix_estimated,2, 0);
MATD_EL(pix_estimated,1,0) /= MATD_EL(pix_estimated,2, 0);
vx_buffer_add_back(buf,
vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_chain(vxo_mat_translate3(MATD_EL(pix_estimated,0,0), MATD_EL(pix_estimated,1,0), 0),
vxo_mat_scale(2.0),
vxo_circle(vxo_mesh_style(vx_green)))));
// create endpoints for lines
loc_t pos;
zarray_get(pix_found, i, &pos); //
points[6*i + 0] = pos.x;
points[6*i + 1] = pos.y;
points[6*i + 2] = 0;
points[6*i + 3] = MATD_EL(pix_estimated,0,0);
points[6*i + 4] = MATD_EL(pix_estimated,1,0);
points[6*i + 5] = 0;
}
// make lines
vx_resc_t *verts = vx_resc_copyf(points, npoints*3);
vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_lines(verts, npoints, GL_LINES,
vxo_points_style(vx_blue, 2.0f))));
}
void * camera_loop(void * data)
{
state_t * state = data;
sleep(2); // wait for 2 seconds before starting the animation
matd_t * zaxis = matd_create(3,1);
zaxis->data[2] = 1;
vx_buffer_add_back(vx_world_get_buffer(state->world, "cam-circle"), vxo_chain(vxo_mat_scale(CAM_RADIUS),
vxo_circle(vxo_lines_style(vx_green, 3))));
vx_buffer_swap(vx_world_get_buffer(state->world, "cam-circle"));
int64_t start_mtime = vx_util_mtime();
// tell each layer to follow
pthread_mutex_lock(&state->mutex);
{
zhash_iterator_t itr;
zhash_iterator_init(state->layers, &itr);
vx_display_t * key;
vx_layer_t * vl;
while(zhash_iterator_next(&itr, &key, &vl)) {
if (1) {
float eye3[] = {CAM_RADIUS,-CAM_RADIUS,45.0f};
float lookat3[] = {CAM_RADIUS,0,0.0f};
float up3[] = {0,1,0};
vx_layer_camera_lookat(vl, eye3, lookat3, up3, 0);
}
}
}
pthread_mutex_unlock(&state->mutex);
while (state->running) {
// 5 seconds revolutions
double rad = ( (vx_util_mtime() - start_mtime) % 5000) * 2* M_PI / 5e3;
// compute the current position and orientation of the "robot"
matd_t * orientation = matd_angle_axis_to_quat(rad, zaxis);
matd_t * pos = matd_create(3,1);
pos->data[0] = cos(rad) * CAM_RADIUS;
pos->data[1] = sin(rad) * CAM_RADIUS;
// tell each layer to follow
pthread_mutex_lock(&state->mutex);
{
zhash_iterator_t itr;
zhash_iterator_init(state->layers, &itr);
vx_display_t * key;
vx_layer_t * vl;
while(zhash_iterator_next(&itr, &key, &vl)) {
vx_layer_camera_follow(vl, pos->data, orientation->data, 1);
}
}
pthread_mutex_unlock(&state->mutex);
vx_buffer_add_back(vx_world_get_buffer(state->world, "robot-proxy"),
vxo_chain(vxo_mat_quat_pos(orientation->data, pos->data),
vxo_box(vxo_lines_style(vx_purple, 3))));
vx_buffer_swap(vx_world_get_buffer(state->world, "robot-proxy"));
matd_destroy(orientation);
matd_destroy(pos);
usleep(100000);
}
matd_destroy(zaxis);
return NULL;
}
