void homography_project(const matd_t *H, double x, double y, double *ox, double *oy) { double xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2); double yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2); double zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2); *ox = xx / zz; *oy = yy / zz; }
matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy) { // Note that every variable that we compute is proportional to the scale factor of H. double R20 = MATD_EL(H, 2, 0); double R21 = MATD_EL(H, 2, 1); double TZ = MATD_EL(H, 2, 2); double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx; double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx; double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx; double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy; double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy; double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy; // compute the scale by requiring that the rotation columns are unit length // (Use geometric average of the two length vectors we have) double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20); double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21); double s = 1.0 / sqrtf(length1 * length2); // get sign of S by requiring the tag to be behind the camera. if (TZ > 0) s *= -1; R20 *= s; R21 *= s; TZ *= s; R00 *= s; R01 *= s; TX *= s; R10 *= s; R11 *= s; TY *= s; // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns. double R02 = R10*R21 - R20*R11; double R12 = R20*R01 - R00*R21; double R22 = R00*R11 - R10*R01; // Improve rotation matrix by applying polar decomposition. if (1) { // do polar decomposition. This makes the rotation matrix // "proper", but probably increases the reprojection error. An // iterative alignment step would be superior. matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02, R10, R11, R12, R20, R21, R22 });
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)))); }
line_t linear_regression(Blob<Gradient> &blob) { line_t line; //Perform linear regression //Ax = B //A = [1 X_0],[1 X_1] //B = [Y_0] ,[Y_1] //x = [ b, m] matd_t *A = matd_create(blob.size(),2); matd_t *B = matd_create(blob.size(),1); int leftmost, rightmost; leftmost = rightmost = blob.getPos(0).x; //Convert to Matd for(size_t i = 0; i < blob.size(); ++i) { pos_t tmp = blob.getPos(i); if(tmp.x < leftmost) { leftmost = tmp.x; } if(tmp.x > rightmost) { rightmost = tmp.x; } MATD_EL(A, i, 0) = 1; MATD_EL(A, i, 1) = tmp.x; MATD_EL(B, i, 0) = tmp.y; } matd_t *x = matd_op("(M' * M)^-1 * M' * M", A,A,A,B); line.b = MATD_EL(x,0,0); line.m = MATD_EL(x,1,0); line.ll.x = leftmost; line.ll.y = line.m*leftmost + line.b; line.ru.x = rightmost; line.ru.y = line.m*rightmost + line.b; line.num_pts = blob.size(); //Compute difference of hypothetical vs. real matd_t *diff = matd_op("(M*M) - M", A,x,B); //Sum of squares matd_t *var = matd_op("M' * M",diff,diff); line.variance = MATD_EL(var,0,0); //Clean up matd_destroy(A); matd_destroy(B); matd_destroy(x); matd_destroy(diff); matd_destroy(var); return line; }
// correspondences is a list of float[4]s, consisting of the points x // and y concatenated. We will compute a homography such that y = Hx matd_t *homography_compute(zarray_t *correspondences, int flags) { // compute centroids of both sets of points (yields a better // conditioned information matrix) double x_cx = 0, x_cy = 0; double y_cx = 0, y_cy = 0; for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); x_cx += c[0]; x_cy += c[1]; y_cx += c[2]; y_cy += c[3]; } int sz = zarray_size(correspondences); x_cx /= sz; x_cy /= sz; y_cx /= sz; y_cy /= sz; // NB We don't normalize scale; it seems implausible that it could // possibly make any difference given the dynamic range of IEEE // doubles. matd_t *A = matd_create(9,9); for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); // (below world is "x", and image is "y") double worldx = c[0] - x_cx; double worldy = c[1] - x_cy; double imagex = c[2] - y_cx; double imagey = c[3] - y_cy; double a03 = -worldx; double a04 = -worldy; double a05 = -1; double a06 = worldx*imagey; double a07 = worldy*imagey; double a08 = imagey; MATD_EL(A, 3, 3) += a03*a03; MATD_EL(A, 3, 4) += a03*a04; MATD_EL(A, 3, 5) += a03*a05; MATD_EL(A, 3, 6) += a03*a06; MATD_EL(A, 3, 7) += a03*a07; MATD_EL(A, 3, 8) += a03*a08; MATD_EL(A, 4, 4) += a04*a04; MATD_EL(A, 4, 5) += a04*a05; MATD_EL(A, 4, 6) += a04*a06; MATD_EL(A, 4, 7) += a04*a07; MATD_EL(A, 4, 8) += a04*a08; MATD_EL(A, 5, 5) += a05*a05; MATD_EL(A, 5, 6) += a05*a06; MATD_EL(A, 5, 7) += a05*a07; MATD_EL(A, 5, 8) += a05*a08; MATD_EL(A, 6, 6) += a06*a06; MATD_EL(A, 6, 7) += a06*a07; MATD_EL(A, 6, 8) += a06*a08; MATD_EL(A, 7, 7) += a07*a07; MATD_EL(A, 7, 8) += a07*a08; MATD_EL(A, 8, 8) += a08*a08; double a10 = worldx; double a11 = worldy; double a12 = 1; double a16 = -worldx*imagex; double a17 = -worldy*imagex; double a18 = -imagex; MATD_EL(A, 0, 0) += a10*a10; MATD_EL(A, 0, 1) += a10*a11; MATD_EL(A, 0, 2) += a10*a12; MATD_EL(A, 0, 6) += a10*a16; MATD_EL(A, 0, 7) += a10*a17; MATD_EL(A, 0, 8) += a10*a18; MATD_EL(A, 1, 1) += a11*a11; MATD_EL(A, 1, 2) += a11*a12; MATD_EL(A, 1, 6) += a11*a16; MATD_EL(A, 1, 7) += a11*a17; MATD_EL(A, 1, 8) += a11*a18; MATD_EL(A, 2, 2) += a12*a12; MATD_EL(A, 2, 6) += a12*a16; MATD_EL(A, 2, 7) += a12*a17; MATD_EL(A, 2, 8) += a12*a18; MATD_EL(A, 6, 6) += a16*a16; MATD_EL(A, 6, 7) += a16*a17; MATD_EL(A, 6, 8) += a16*a18; MATD_EL(A, 7, 7) += a17*a17; MATD_EL(A, 7, 8) += a17*a18; MATD_EL(A, 8, 8) += a18*a18; double a20 = -worldx*imagey; double a21 = -worldy*imagey; double a22 = -imagey; double a23 = worldx*imagex; double a24 = worldy*imagex; double a25 = imagex; MATD_EL(A, 0, 0) += a20*a20; MATD_EL(A, 0, 1) += a20*a21; MATD_EL(A, 0, 2) += a20*a22; MATD_EL(A, 0, 3) += a20*a23; MATD_EL(A, 0, 4) += a20*a24; MATD_EL(A, 0, 5) += a20*a25; MATD_EL(A, 1, 1) += a21*a21; MATD_EL(A, 1, 2) += a21*a22; MATD_EL(A, 1, 3) += a21*a23; MATD_EL(A, 1, 4) += a21*a24; MATD_EL(A, 1, 5) += a21*a25; MATD_EL(A, 2, 2) += a22*a22; MATD_EL(A, 2, 3) += a22*a23; MATD_EL(A, 2, 4) += a22*a24; MATD_EL(A, 2, 5) += a22*a25; MATD_EL(A, 3, 3) += a23*a23; MATD_EL(A, 3, 4) += a23*a24; MATD_EL(A, 3, 5) += a23*a25; MATD_EL(A, 4, 4) += a24*a24; MATD_EL(A, 4, 5) += a24*a25; MATD_EL(A, 5, 5) += a25*a25; } // make symmetric for (int i = 0; i < 9; i++) for (int j = i+1; j < 9; j++) MATD_EL(A, j, i) = MATD_EL(A, i, j); matd_t *H = matd_create(3,3); if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) { // compute singular vector by (carefully) inverting the rank-deficient matrix. if (1) { matd_t *Ainv = matd_inverse(A); double scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; matd_destroy(Ainv); } else { matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 }); matd_t *Ainv = NULL; if (0) { matd_lu_t *lu = matd_lu(A); Ainv = matd_lu_solve(lu, b); matd_lu_destroy(lu); } else { matd_chol_t *chol = matd_chol(A); Ainv = matd_chol_solve(chol, b); matd_chol_destroy(chol); } double scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; matd_destroy(b); matd_destroy(Ainv); } } else {
// correspondences is a list of float[4]s, consisting of the points x // and y concatenated. We will compute a homography such that y = Hx matd_t *homography_compute(zarray_t *correspondences) { // compute centroids of both sets of points (yields a better // conditioned information matrix) double x_cx = 0, x_cy = 0; double y_cx = 0, y_cy = 0; for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); x_cx += c[0]; x_cy += c[1]; y_cx += c[2]; y_cy += c[3]; } int sz = zarray_size(correspondences); x_cx /= sz; x_cy /= sz; y_cx /= sz; y_cy /= sz; // NB We don't normalize scale; it seems implausible that it could // possibly make any difference given the dynamic range of IEEE // doubles. matd_t *A = matd_create(9,9); for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); // (below world is "x", and image is "y") double worldx = c[0] - x_cx; double worldy = c[1] - x_cy; double imagex = c[2] - y_cx; double imagey = c[3] - y_cy; double a03 = -worldx; double a04 = -worldy; double a05 = -1; double a06 = worldx*imagey; double a07 = worldy*imagey; double a08 = imagey; MATD_EL(A, 3, 3) += a03*a03; MATD_EL(A, 3, 4) += a03*a04; MATD_EL(A, 3, 5) += a03*a05; MATD_EL(A, 3, 6) += a03*a06; MATD_EL(A, 3, 7) += a03*a07; MATD_EL(A, 3, 8) += a03*a08; MATD_EL(A, 4, 4) += a04*a04; MATD_EL(A, 4, 5) += a04*a05; MATD_EL(A, 4, 6) += a04*a06; MATD_EL(A, 4, 7) += a04*a07; MATD_EL(A, 4, 8) += a04*a08; MATD_EL(A, 5, 5) += a05*a05; MATD_EL(A, 5, 6) += a05*a06; MATD_EL(A, 5, 7) += a05*a07; MATD_EL(A, 5, 8) += a05*a08; MATD_EL(A, 6, 6) += a06*a06; MATD_EL(A, 6, 7) += a06*a07; MATD_EL(A, 6, 8) += a06*a08; MATD_EL(A, 7, 7) += a07*a07; MATD_EL(A, 7, 8) += a07*a08; MATD_EL(A, 8, 8) += a08*a08; double a10 = worldx; double a11 = worldy; double a12 = 1; double a16 = -worldx*imagex; double a17 = -worldy*imagex; double a18 = -imagex; MATD_EL(A, 0, 0) += a10*a10; MATD_EL(A, 0, 1) += a10*a11; MATD_EL(A, 0, 2) += a10*a12; MATD_EL(A, 0, 6) += a10*a16; MATD_EL(A, 0, 7) += a10*a17; MATD_EL(A, 0, 8) += a10*a18; MATD_EL(A, 1, 1) += a11*a11; MATD_EL(A, 1, 2) += a11*a12; MATD_EL(A, 1, 6) += a11*a16; MATD_EL(A, 1, 7) += a11*a17; MATD_EL(A, 1, 8) += a11*a18; MATD_EL(A, 2, 2) += a12*a12; MATD_EL(A, 2, 6) += a12*a16; MATD_EL(A, 2, 7) += a12*a17; MATD_EL(A, 2, 8) += a12*a18; MATD_EL(A, 6, 6) += a16*a16; MATD_EL(A, 6, 7) += a16*a17; MATD_EL(A, 6, 8) += a16*a18; MATD_EL(A, 7, 7) += a17*a17; MATD_EL(A, 7, 8) += a17*a18; MATD_EL(A, 8, 8) += a18*a18; double a20 = -worldx*imagey; double a21 = -worldy*imagey; double a22 = -imagey; double a23 = worldx*imagex; double a24 = worldy*imagex; double a25 = imagex; MATD_EL(A, 0, 0) += a20*a20; MATD_EL(A, 0, 1) += a20*a21; MATD_EL(A, 0, 2) += a20*a22; MATD_EL(A, 0, 3) += a20*a23; MATD_EL(A, 0, 4) += a20*a24; MATD_EL(A, 0, 5) += a20*a25; MATD_EL(A, 1, 1) += a21*a21; MATD_EL(A, 1, 2) += a21*a22; MATD_EL(A, 1, 3) += a21*a23; MATD_EL(A, 1, 4) += a21*a24; MATD_EL(A, 1, 5) += a21*a25; MATD_EL(A, 2, 2) += a22*a22; MATD_EL(A, 2, 3) += a22*a23; MATD_EL(A, 2, 4) += a22*a24; MATD_EL(A, 2, 5) += a22*a25; MATD_EL(A, 3, 3) += a23*a23; MATD_EL(A, 3, 4) += a23*a24; MATD_EL(A, 3, 5) += a23*a25; MATD_EL(A, 4, 4) += a24*a24; MATD_EL(A, 4, 5) += a24*a25; MATD_EL(A, 5, 5) += a25*a25; } // make symmetric for (int i = 0; i < 9; i++) for (int j = i+1; j < 9; j++) MATD_EL(A, j, i) = MATD_EL(A, i, j); matd_svd_t svd = matd_svd(A); matd_t *Ainv = matd_inverse(A); double scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); if (1) { // compute singular vector using SVD. A bit slower, but more accurate. matd_svd_t svd = matd_svd(A); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) // MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale; MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); } else { // compute singular vector by (carefully) inverting the rank-deficient matrix. matd_t *Ainv = matd_inverse(A); double scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale; matd_destroy(Ainv); } matd_t *Tx = matd_identity(3); MATD_EL(Tx,0,2) = -x_cx; MATD_EL(Tx,1,2) = -x_cy; matd_t *Ty = matd_identity(3); MATD_EL(Ty,0,2) = y_cx; MATD_EL(Ty,1,2) = y_cy; matd_t *H2 = matd_op("M*M*M", Ty, H, Tx); matd_destroy(A); matd_destroy(Tx); matd_destroy(Ty); matd_destroy(H); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); return H2; }
// iterative alignment step would be superior. matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02, R10, R11, R12, R20, R21, R22 }); matd_svd_t svd = matd_svd(R); matd_destroy(R); R = matd_op("M*M'", svd.U, svd.V); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); R00 = MATD_EL(R, 0, 0); R01 = MATD_EL(R, 0, 1); R02 = MATD_EL(R, 0, 2); R10 = MATD_EL(R, 1, 0); R11 = MATD_EL(R, 1, 1); R12 = MATD_EL(R, 1, 2); R20 = MATD_EL(R, 2, 0); R21 = MATD_EL(R, 2, 1); R22 = MATD_EL(R, 2, 2); matd_destroy(R); } return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX, R10, R11, R12, TY, R20, R21, R22, TZ,
// 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); }