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);
