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