int get_slope(loc_t a , loc_t b, int add_line, int reverse, vx_buffer_t* buf)
{
if(add_line) {
int npoints = 2;
float points[npoints*3];
points[0] = a.x;
points[1] = a.y;
points[2] = 0;
points[3] = b.x;
points[4] = b.y;
points[5] = 0;
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))));
}
if(reverse) {
if((a.y - b.y) == 0)
return((a.x - b.x)/.0001);
int ret = (a.x - b.x) / (a.y - b.y);
return(ret);
}
if((a.x - b.x) == 0)
return((a.y - b.y)/.0001);
int ret = (a.y - b.y) / (a.x - b.x);
return(ret);
}
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 add_line_to_buffer(image_u32_t* im, vx_buffer_t* buf, double size,
loc_t p1, loc_t p2, const float* color)
{
int npoints = 2; // line per chart blob
float points[npoints*3];
points[0] = p1.x;
points[1] = p1.y;
points[2] = 0;
points[3] = p2.x;
points[4] = p2.y;
points[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(color, size))));
}
static void draw(state_t * state, vx_world_t * world)
{
if (1) {
vx_buffer_add_back(vx_world_get_buffer(world, "grid"),
vxo_grid());
vx_buffer_set_draw_order(vx_world_get_buffer(world, "grid"), -100);
vx_buffer_swap(vx_world_get_buffer(world, "grid"));
}
// Draw from the vx shape library
if (1) {
vx_buffer_add_back(vx_world_get_buffer(world, "fixed-cube"),
vxo_chain(vxo_mat_translate3(3.0,0,0),
vxo_mat_scale3(2,2,2),
/* vxo_box(vxo_mesh_style(vx_orange)))); */
vxo_box(vxo_mesh_style_fancy(vx_orange, vx_orange, vx_white, 1.0, 400.0, 2))));
vx_buffer_add_back(vx_world_get_buffer(world, "fixed-cube"),
vxo_chain(vxo_mat_translate3(0,3.0,0),
vxo_mat_scale3(1,1,1),
vxo_depth_test(0,
vxo_box(vxo_mesh_style_solid(vx_green)))));
vx_buffer_swap(vx_world_get_buffer(world, "fixed-cube"));
}
if (1) {
float Tr = .2;
float amb[] = {0.0,0.0,0.0};
float diff[] = {0.0,0.0,0.0};
float spec[] = {1.0,1.0,1.0};
float specularity = 1.0;
int type = 2;
vx_buffer_add_back(vx_world_get_buffer(world, "window"),
vxo_chain(vxo_mat_translate3(0,0,2.5),
vxo_mat_rotate_y(-M_PI/7),
vxo_mat_rotate_z(M_PI/5),
vxo_mat_rotate_x(M_PI/2),
vxo_mat_scale3(10,10,1),
vxo_rect(vxo_mesh_style_fancy(amb, diff, spec, Tr, specularity, type),
vxo_lines_style(vx_black,2))));
vx_buffer_swap(vx_world_get_buffer(world, "window"));
vx_buffer_set_draw_order(vx_world_get_buffer(world, "window"), 100);
}
if (1) {
// Draw a custom ellipse:
int npoints = 35;
float points[npoints*3];
for (int i = 0; i < npoints; i++) {
float angle = 2*M_PI*i/npoints;
float x = 5.0f*cosf(angle);
float y = 8.0f*sinf(angle);
float z = 0.0f;
points[3*i + 0] = x;
points[3*i + 1] = y;
points[3*i + 2] = z;
}
vx_buffer_add_back(vx_world_get_buffer(world, "ellipse"), vxo_lines(vx_resc_copyf (points, npoints*3),
npoints, GL_LINE_LOOP,
vxo_lines_style(vx_purple, 1.0f) ));
vx_buffer_swap(vx_world_get_buffer(world, "ellipse"));
}
if (1) {
vx_object_t *vt = vxo_text_create(VXO_TEXT_ANCHOR_TOP_RIGHT, "<<right,#0000ff>>Heads Up!\n");
vx_buffer_t *vb = vx_world_get_buffer(world, "text");
vx_buffer_add_back(vb, vxo_pix_coords(VX_ORIGIN_TOP_RIGHT,vt));
vx_buffer_swap(vb);
}
// Draw a texture
if (state->img != NULL){
image_u32_t * img = state->img;
vx_object_t * o3 = vxo_image_texflags(vx_resc_copyui(img->buf, img->stride*img->height),
img->width, img->height, img->stride,
GL_RGBA, VXO_IMAGE_FLIPY,
VX_TEX_MIN_FILTER | VX_TEX_MAG_FILTER);
// pack the image into the unit square
vx_buffer_t * vb = vx_world_get_buffer(world, "texture");
vx_buffer_add_back(vb,vxo_chain(
vxo_mat_scale(1.0/img->height),
vxo_mat_translate3(0, - img->height, 0),
o3));
vx_buffer_swap(vb);
}
}
// 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);
}
void connect_lines(zarray_t* locs, vx_buffer_t* buf)
{
connection_t lines[12];
int idx = 0;
// iterate
for(int i = 0; i < zarray_size(locs); i++) {
loc_t curr;
zarray_get(locs, i, &curr);
for(int j = 0; j < zarray_size(locs); j++) {
// make a connection to every point except self and add to the lines
loc_t other;
zarray_get(locs, j, &other);
if(i != j) { // not same point
lines[idx].start = curr;
lines[idx].end = other;
lines[idx].length = sqrt((curr.x - other.x)*(curr.x - other.x)
+ (curr.y - other.y)*(curr.y - other.y));
idx++;
}
}
}
// sort the array by distance
int flag = 1;
connection_t tmp;
for(int i = 0; (i < 12) && flag; i++)
{
flag = 0; // only go over again if swap has been made
for(int j = 0; j < (11); j++)
{
if(lines[j+1].length < lines[j].length)
{
tmp = lines[j];
lines[j] = lines[j+1];
lines[j+1] = tmp;
flag = 1;
}
}
}
for(int i = 0; (i < 12); i++)
{
// printf("%lf\n", lines[i].length);
}
// usleep(2000000);
int npoints = 8;
float points[npoints*3];
for(int i = 0; i < 4; i++) {
points[6*i + 0] = lines[i*2].start.x;
points[6*i + 1] = lines[i*2].start.y;
points[6*i + 2] = 1;
points[6*i + 3] = lines[i*2].end.x;
points[6*i + 4] = lines[i*2].end.y;
points[6*i + 5] = 1;
}
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))));
}
