void vx_util_unproject(double * winxyz, double * model_matrix, double * projection_matrix, int * viewport, double * vec3_out) { zarray_t * fp = zarray_create(sizeof(matd_t*)); matd_t * mm = matd_create_data(4, 4, model_matrix); zarray_add(fp, &mm); matd_t * pm = matd_create_data(4, 4, projection_matrix); zarray_add(fp, &pm); matd_t *invpm = matd_op("(MM)^-1", pm, mm); zarray_add(fp, &invpm); double v[4] = { 2*(winxyz[0]-viewport[0]) / viewport[2] - 1, 2*(winxyz[1]-viewport[1]) / viewport[3] - 1, 2*winxyz[2] - 1, 1 }; matd_t * vm = matd_create_data(4, 1, v); zarray_add(fp, &vm); matd_t * objxyzh = matd_op("MM", invpm, vm); zarray_add(fp, &objxyzh); vec3_out[0] = objxyzh->data[0] / objxyzh->data[3]; vec3_out[1] = objxyzh->data[1] / objxyzh->data[3]; vec3_out[2] = objxyzh->data[2] / objxyzh->data[3]; // cleanup zarray_vmap(fp, matd_destroy); zarray_destroy(fp); }
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; }
void vx_util_project(double * xyz, double * M44, double * P44, int * viewport, double * win_out3) { zarray_t * fp = zarray_create(sizeof(matd_t*)); matd_t * M = matd_create_data(4,4, M44); zarray_add(fp, &M); matd_t * P = matd_create_data(4,4, P44); zarray_add(fp, &P); matd_t * xyzp = matd_create(4,1); zarray_add(fp, &xyzp); memcpy(xyzp->data, xyz, 3*sizeof(double)); xyzp->data[3] = 1.0; matd_t * p = matd_op("MMM", P, M, xyzp); zarray_add(fp, &p); p->data[0] = p->data[0] / p->data[3]; p->data[1] = p->data[1] / p->data[3]; p->data[2] = p->data[2] / p->data[3]; double res[] = { viewport[0] + viewport[2]*(p->data[0]+1)/2.0, viewport[1] + viewport[3]*(p->data[1]+1)/2.0, (viewport[2] + 1)/2.0 }; memcpy(win_out3, res, 3*sizeof(double)); // cleanup zarray_vmap(fp, matd_destroy); zarray_destroy(fp); }
void vx_util_lookat(double * _eye, double * _lookat, double * _up, double * _out44) { zarray_t * fp = zarray_create(sizeof(matd_t*)); matd_t * eye = matd_create_data(3,1, _eye); zarray_add(fp, &eye); matd_t * lookat = matd_create_data(3,1, _lookat); zarray_add(fp, &lookat); matd_t * up = matd_create_data(3,1, _up); zarray_add(fp, &up); up = matd_vec_normalize(up); zarray_add(fp, &up); // note different pointer than before! matd_t * tmp1 = matd_subtract(lookat, eye); zarray_add(fp, &tmp1); matd_t * f = matd_vec_normalize(tmp1); zarray_add(fp, &f); matd_t * s = matd_crossproduct(f, up); zarray_add(fp, &s); matd_t * u = matd_crossproduct(s, f); zarray_add(fp, &u); matd_t * M = matd_create(4,4); // set the rows of M with s, u, -f zarray_add(fp, &M); memcpy(M->data,s->data,3*sizeof(double)); memcpy(M->data + 4,u->data,3*sizeof(double)); memcpy(M->data + 8,f->data,3*sizeof(double)); for (int i = 0; i < 3; i++) M->data[2*4 +i] *= -1; M->data[3*4 + 3] = 1.0; matd_t * T = matd_create(4,4); T->data[0*4 + 3] = -eye->data[0]; T->data[1*4 + 3] = -eye->data[1]; T->data[2*4 + 3] = -eye->data[2]; T->data[0*4 + 0] = 1; T->data[1*4 + 1] = 1; T->data[2*4 + 2] = 1; T->data[3*4 + 3] = 1; zarray_add(fp, &T); matd_t * MT = matd_op("MM",M,T); zarray_add(fp, &MT); memcpy(_out44, MT->data, 16*sizeof(double)); // cleanup zarray_vmap(fp, matd_destroy); zarray_destroy(fp); }
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)))); }
vx_camera_pos_t * default_cam_mgr_get_cam_pos(default_cam_mgr_t * state, int * viewport, uint64_t mtime) { vx_camera_pos_t * p = calloc(1, sizeof(vx_camera_pos_t)); memcpy(p->viewport, viewport, 4*sizeof(int)); p->perspective_fovy_degrees = state->perspective_fovy_degrees; p->zclip_near = state->zclip_near; p->zclip_far = state->zclip_far; // process a fit command if necessary: if (state->fit != NULL) { fit_t * f = state->fit; // consume the fit command state->fit = NULL; // XXX minor race condition, could lose a fit cmd // XXX We can probably do better than this using the viewport... state->lookat1[0] = (f->xy0[0] + f->xy1[0]) / 2; state->lookat1[1] = (f->xy0[1] + f->xy1[1]) / 2; state->lookat1[2] = 0; // dimensions of fit double Fw = f->xy1[0] - f->xy0[0]; double Fh = f->xy1[1] - f->xy0[1]; // aspect ratios double Far = Fw / Fh; double Var = p->viewport[2] * 1.0 / p->viewport[3]; double tAngle = tan(p->perspective_fovy_degrees/2*M_PI/180.0); double height = fabs(0.5 * (Var > Far ? Fh : Fw / Var) / tAngle); state->eye1[0] = state->lookat1[0]; state->eye1[1] = state->lookat1[1]; state->eye1[2] = height; state->up1[0] = 0; state->up1[1] = 1; state->up1[2] = 0; state->mtime1 = f->mtime; free(f); } if (mtime > state->mtime1) { memcpy(p->eye, state->eye1, 3*sizeof(double)); memcpy(p->up, state->up1, 3*sizeof(double)); memcpy(p->lookat, state->lookat1, 3*sizeof(double)); p->perspectiveness = state->perspectiveness1; } else if (mtime <= state->mtime0) { memcpy(p->eye, state->eye0, 3*sizeof(double)); memcpy(p->up, state->up0, 3*sizeof(double)); memcpy(p->lookat, state->lookat0, 3*sizeof(double)); p->perspectiveness = state->perspectiveness0; } else { double alpha1 = ((double) mtime - state->mtime0) / (state->mtime1 - state->mtime0); double alpha0 = 1.0 - alpha1; scaled_combination(state->eye0, alpha0, state->eye1, alpha1, p->eye, 3); scaled_combination(state->up0, alpha0, state->up1, alpha1, p->up, 3); scaled_combination(state->lookat0, alpha0, state->lookat1, alpha1, p->lookat, 3); p->perspectiveness = state->perspectiveness0*alpha0 + state->perspectiveness1*alpha1; // Tweak so eye-to-lookat is the right distance { zarray_t * fp = zarray_create(sizeof(matd_t*)); matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye); matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat); matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up); matd_t * eye0 = matd_create_data(3,1, state->eye0); zarray_add(fp, &eye0); matd_t * lookat0 = matd_create_data(3,1, state->lookat0); zarray_add(fp, &lookat0); matd_t * up0 = matd_create_data(3,1, state->up0); zarray_add(fp, &up0); matd_t * eye1 = matd_create_data(3,1, state->eye1); zarray_add(fp, &eye1); matd_t * lookat1 = matd_create_data(3,1, state->lookat1); zarray_add(fp, &lookat1); matd_t * up1 = matd_create_data(3,1, state->up1); zarray_add(fp, &up1); double dist0 = matd_vec_dist(eye0, lookat0); double dist1 = matd_vec_dist(eye1, lookat1); matd_t * dist = matd_create_scalar(dist0*alpha0 + dist1*alpha1); zarray_add(fp, &dist); matd_t * eye2p = matd_subtract(eye,lookat); zarray_add(fp, &eye2p); eye2p = matd_vec_normalize(eye2p); zarray_add(fp, &eye2p); eye = matd_op("M + (M*M)", lookat, eye2p, dist); // Only modified eye memcpy(p->eye, eye->data, 3*sizeof(double)); zarray_vmap(fp, matd_destroy); zarray_destroy(fp); } } // Need to do more fixup depending on interface mode! { if (state->interface_mode <= 2.0) { // stack eye on lookat: p->eye[0] = p->lookat[0]; p->eye[1] = p->lookat[1]; p->lookat[2] = 0; // skip fabs() for ENU/NED compat //p->eye[2] = fabs(p->eye[2]); { matd_t * up = matd_create_data(3,1, p->up); up->data[2] = 0; // up should never point in Z matd_t * up_norm = matd_vec_normalize(up); memcpy(p->up, up_norm->data, sizeof(double)*3); matd_destroy(up); matd_destroy(up_norm); } } else if (state->interface_mode == 2.5) { zarray_t * fp = zarray_create(sizeof(matd_t*)); matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye); matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat); matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up); lookat->data[2] = 0.0; // Level horizon matd_t * dir = matd_subtract(lookat, eye); zarray_add(fp, &dir); matd_t * dir_norm = matd_vec_normalize(dir); zarray_add(fp, &dir_norm); matd_t * left = matd_crossproduct(up, dir_norm); zarray_add(fp, &left); left->data[2] = 0.0; left = matd_vec_normalize(left); zarray_add(fp, &left); // Don't allow upside down //up->data[2] = fmax(0.0, up->data[2]); // XXX NED? // Find an 'up' direction perpendicular to left matd_t * dot_scalar = matd_create_scalar(matd_vec_dot_product(up, left)); zarray_add(fp, &dot_scalar); up = matd_op("M - (M*M)", up, left, dot_scalar); zarray_add(fp, &up); up = matd_vec_normalize(up); zarray_add(fp, &up); // Now find eye position by computing new lookat dir matd_t * eye_dir = matd_crossproduct(up, left); zarray_add(fp, &eye_dir); matd_t *eye_dist_scalar = matd_create_scalar(matd_vec_dist(eye, lookat)); zarray_add(fp, &eye_dist_scalar); eye = matd_op("M + (M*M)", lookat, eye_dir, eye_dist_scalar); zarray_add(fp, &eye); // export results back to p: memcpy(p->eye, eye->data, sizeof(double)*3); memcpy(p->lookat, lookat->data, sizeof(double)*3); memcpy(p->up, up->data, sizeof(double)*3); zarray_vmap(fp, matd_destroy); zarray_destroy(fp); } } // Fix up for bad zoom if (1) { matd_t * eye = matd_create_data(3,1, p->eye); matd_t * lookat = matd_create_data(3,1, p->lookat); matd_t * up = matd_create_data(3,1, p->up); matd_t * lookeye = matd_subtract(lookat, eye); matd_t * lookdir = matd_vec_normalize(lookeye); double dist = matd_vec_dist(eye, lookat); dist = fmin(state->zclip_far / 3.0, dist); dist = fmax(state->zclip_near * 3.0, dist); matd_scale_inplace(lookdir, dist); matd_t * eye_fixed = matd_subtract(lookat, lookdir); memcpy(p->eye, eye_fixed->data, sizeof(double)*3); matd_destroy(eye); matd_destroy(lookat); matd_destroy(up); matd_destroy(lookeye); matd_destroy(lookdir); matd_destroy(eye_fixed); } // copy the result back into 'state' { memcpy(state->eye0, p->eye, 3*sizeof(double)); memcpy(state->up0, p->up, 3*sizeof(double)); memcpy(state->lookat0, p->lookat, 3*sizeof(double)); state->perspectiveness0 = p->perspectiveness; state->mtime0 = mtime; } return p; }
// 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; }
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 }); 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);