inline Matrix *
fminvert (const Matrix *other) {
Matrix *result;
float det;
if (other->rows == 2 && other->cols == 2) {
result = fmatrix(2,2);
fmset(result,0,0, *fmget(other,1,1));
fmset(result,0,1, *fmget(other,0,1) * -1);
fmset(result,1,0, *fmget(other,1,0) * -1);
fmset(result,1,1, *fmget(other,0,0));
det = (*fmget(other,0,0) * *fmget(other,1,1)) -
(*fmget(other,0,1) * *fmget(other,1,0));
if (det != 0)
fmscaleeq(result, 1.0 / det);
else {
fmfree(result);
result = NULL;
errno = EDOM;
}
}else {
Matrix *id = fmidentity(other->rows);
result = fmsolve(other, id);
fmfree(id);
}
return result;
}
Matrix * Pose::translation4D(float dx, float dy, float dz){
Matrix * trans = identity4D();
fmset(trans,X_AXIS,W_AXIS,dx);
fmset(trans,Y_AXIS,W_AXIS,dy);
fmset(trans,Z_AXIS,W_AXIS,dz);
return trans;
}
inline Matrix *
solveLU (const LUDecomposition lud, const Matrix *B) {
Matrix *X;
int m, n, nx, i, j, k;
nx = B->cols;
m = lud.LU->rows;
n = lud.LU->cols;
X = fmgetmatrix(B, lud.piv, lud.LU->rows, 0, nx);
assert(X);
k = 0;
while (k < n) {
i = k + 1;
while (i < n) {
j = 0;
while (j < nx) {
fmset(X, i, j,
(*fmget(X,i,j) - *fmget(X,k,j) * *fmget(lud.LU,i,k)));
j++;
}
i++;
}
k++;
}
k = n - 1;
while (k >= 0) {
j = 0;
while (j < nx) {
fmset(X, k, j, *fmget(X,k,j) / *fmget(lud.LU,k,k));
j++;
}
i = 0;
while (i < k) {
j = 0;
while (j < nx) {
fmset(X, i, j,
(*fmget(X,i,j) - *fmget(X,k,j) * *fmget(lud.LU,i,k)));
j++;
}
i++;
}
k--;
}
return X;
}
inline Matrix *
fmsub (const Matrix *m1, const Matrix *m2) {
Matrix *m;
int r, c;
if (m1->rows != m2->rows || m1->cols != m2->cols) {
errno = EINVAL;
return NULL;
}
m = fmatrix(m1->rows, m1->cols);
if (m == NULL)
return NULL;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, *fmget(m1,r,c) - *fmget(m2,r,c));
c++;
}
r++;
}
return m;
}
inline Matrix *
fmmul (const Matrix *m1, const Matrix *m2) {
Matrix *m;
int r, c, n;
float sum;
if (m1->cols != m2->rows) {
errno = EINVAL;
return NULL;
}
m = fmatrix(m1->rows, m2->cols);
if (m == NULL)
return NULL;
r = 0;
while (r < m1->rows) {
c = 0;
while (c < m2->cols) {
sum = 0;
n = 0;
while (n < m1->cols) {
sum += *fmget(m1, r, n) * *fmget(m2, n, c);
n++;
}
fmset(m, r, c, sum);
c++;
}
r++;
}
return m;
}
inline void
fmnegateeq (Matrix *m) {
int r, c;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, -(*fmget(m, r, c)));
c++;
}
r++;
}
}
inline Matrix *
fmidentity (int nrows) {
Matrix *m;
int i;
m = fmatrix2(nrows, nrows, 0);
i = 0;
while (i < nrows) {
fmset(m, i, i, 1);
i++;
}
return m;
}
inline void
fmscaleeq (Matrix *m, float scalar) {
int r, c;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, scalar * *fmget(m, r, c));
c++;
}
r++;
}
}
inline Matrix *
fmgetmatrix (const Matrix *other, const int *rows, int nrows, int c1, int c2) {
Matrix *m;
int r, c;
m = fmatrix(nrows, c2 - c1);
if (m == NULL)
return NULL;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, *fmget(other, rows[r], c1 + c));
c++;
}
r++;
}
return m;
}
inline Matrix *
fmscale (const Matrix *other, float scalar) {
Matrix *m;
int r, c;
m = fmatrix(other->rows, other->cols);
if (m == NULL)
return NULL;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, scalar * *fmget(other, r, c));
c++;
}
r++;
}
return m;
}
inline Matrix *
fmtranspose (const Matrix *other) {
Matrix *m;
int r, c;
m = fmatrix(other->cols, other->rows);
if (m == NULL)
return NULL;
r = 0;
while (r < m->rows) {
c = 0;
while (c < m->cols) {
fmset(m, r, c, *fmget(other,c,r));
c++;
}
r++;
}
return m;
}
inline int
fmsubeq (Matrix *m1, const Matrix *m2) {
int r, c;
if (m1->rows != m2->rows || m1->cols != m2->cols) {
errno = EINVAL;
return -1;
}
r = 0;
while (r < m1->rows) {
c = 0;
while (c < m1->rows) {
fmset(m1, r, c, *fmget(m1,r,c) - *fmget(m2,r,c));
c++;
}
r++;
}
return 0;
}
inline int
fmmulstore (const Matrix *m1, const Matrix *m2, Matrix *store) {
int r, c, n;
float sum, *tmp;
if (m1->rows != m2->cols) {
errno = EINVAL;
return -1;
}
if (m1->rows != store->rows || m2->cols != store->cols) {
store->rows = m1->rows;
store->cols = m2->cols;
tmp = (float*)realloc(store->data,
store->rows * store->cols * sizeof(float));
if (tmp == NULL)
return -1;
store->data = tmp;
}
r = 0;
while (r < store->rows) {
c = 0;
while (c < store->cols) {
sum = 0;
n = 0;
while (n < m1->cols) {
sum += *fmget(m1, r, n) * *fmget(m2, n, c);
n++;
}
fmset(store, r, c, sum);
c++;
}
r++;
}
return 0;
}
/*
** general linear regression
*/
VAttrList
VRegression(ListInfo *linfo, int nlists, VShort minval, VImage design, VFloat sigma, VLong itr) {
VAttrList out_list;
VImageInfo *xinfo;
int nbands = 0, nslices = 0, nrows = 0, ncols = 0, slice, row, col, nr, nc;
VImage src[NSLICES], res_image = NULL;
VImage beta_image[MBETA], BCOV = NULL, KX_image = NULL;
VImage res_map[ETMP];
float smooth_fwhm = 0, vx = 0, vy = 0, vz = 0;
VFloat *float_pp, df;
VRepnKind repn;
float d, err;
int i, k, l, n, m = 0, nt, fd = 0, npix = 0;
int i0 = 0, i1 = 0;
double u, sig, trace = 0, trace2 = 0, var = 0, sum = 0, nx = 0, mean = 0, sum2;
float *ptr1, *ptr2;
double x;
gsl_matrix_float *X = NULL, *XInv = NULL, *SX = NULL;
gsl_vector_float *y, *z, *beta, *ys;
gsl_vector *kernel;
gsl_matrix_float *S = NULL, *Vc = NULL, *F = NULL, *P = NULL, *Q = NULL;
gsl_matrix_float *R = NULL, *RV = NULL;
VBoolean smooth = TRUE; /* no smoothness estimation */
gsl_set_error_handler_off();
/*
** read input data
*/
nslices = nbands = nrows = ncols = 0;
for(k = 0; k < nlists; k++) {
n = linfo[k].nslices;
nr = linfo[k].nrows;
nc = linfo[k].ncols;
nt = linfo[k].ntimesteps;
nbands += nt;
if(nslices == 0)
nslices = n;
else if(nslices != n)
VError(" inconsistent image dimensions, slices: %d %d", n, nslices);
if(nrows == 0)
nrows = nr;
else if(nrows != nr)
VError(" inconsistent image dimensions, rows: %d %d", nr, nrows);
if(ncols == 0)
ncols = nc;
else if(ncols != nc)
VError(" inconsistent image dimensions, cols: %d %d", nc, ncols);
}
fprintf(stderr, " num images: %d, image dimensions: %d x %d x %d\n",
nlists, nslices, nrows, ncols);
/*
** get design dimensions
*/
m = VImageNRows(design); /* number of timesteps */
n = VImageNColumns(design); /* number of covariates */
fprintf(stderr, " ntimesteps=%d, num covariates=%d\n", m, n);
if(n >= MBETA)
VError(" too many covariates (%d), max is %d", n, MBETA);
if(m != nbands)
VError(" design dimension inconsistency: %d %d", m, nbands);
fprintf(stderr, " working...\n");
/*
** read design matrix
*/
X = gsl_matrix_float_alloc(m, n);
for(k = 0; k < m; k++) {
for(l = 0; l < n; l++) {
x = VGetPixel(design, 0, k, l);
fmset(X, k, l, (float)x);
}
}
/*
** pre-coloring, set up K-matrix, S=K, V = K*K^T with K=S
*/
S = gsl_matrix_float_alloc(m, m);
GaussMatrix((double)sigma, S);
Vc = fmat_x_matT(S, S, NULL);
/*
** compute pseudoinverse
*/
SX = fmat_x_mat(S, X, NULL);
XInv = fmat_PseudoInv(SX, NULL);
/*
** get variance estimate
*/
Q = fmat_x_mat(XInv, Vc, Q);
F = fmat_x_matT(Q, XInv, F);
BCOV = VCreateImage(1, n, n, VFloatRepn);
float_pp = VImageData(BCOV);
ptr1 = F->data;
for(i = 0; i < n * n; i++)
*float_pp++ = *ptr1++;
gsl_matrix_float_free(Q);
gsl_matrix_float_free(F);
/*
** get effective degrees of freedom
*/
R = gsl_matrix_float_alloc(m, m);
P = fmat_x_mat(SX, XInv, P);
gsl_matrix_float_set_identity(R);
gsl_matrix_float_sub(R, P);
RV = fmat_x_mat(R, Vc, NULL);
trace = 0;
for(i = 0; i < m; i++)
trace += fmget(RV, i, i);
P = fmat_x_mat(RV, RV, P);
trace2 = 0;
for(i = 0; i < m; i++)
trace2 += fmget(P, i, i);
df = (trace * trace) / trace2;
fprintf(stderr, " df= %.3f\n", df);
/*
** create output images
*/
xinfo = linfo[0].info;
out_list = VCreateAttrList();
res_image = VCreateImage(nslices, nrows, ncols, VFloatRepn);
VFillImage(res_image, VAllBands, 0);
VSetAttr(VImageAttrList(res_image), "name", NULL, VStringRepn, "RES/trRV");
VSetAttr(VImageAttrList(res_image), "modality", NULL, VStringRepn, "RES/trRV");
VSetAttr(VImageAttrList(res_image), "df", NULL, VFloatRepn, df);
VSetAttr(VImageAttrList(res_image), "patient", NULL, VStringRepn, xinfo->patient);
VSetAttr(VImageAttrList(res_image), "voxel", NULL, VStringRepn, xinfo->voxel);
VSetAttr(VImageAttrList(res_image), "repetition_time", NULL, VLongRepn, itr);
VSetAttr(VImageAttrList(res_image), "talairach", NULL, VStringRepn, xinfo->talairach);
/* neu */
VSetAttr(VImageAttrList(res_image),"indexOrigin",NULL,VStringRepn,xinfo->indexOrigin);
VSetAttr(VImageAttrList(res_image),"columnVec",NULL,VStringRepn,xinfo->columnVec);
VSetAttr(VImageAttrList(res_image),"rowVec",NULL,VStringRepn,xinfo->rowVec);
VSetAttr(VImageAttrList(res_image),"sliceVec",NULL,VStringRepn,xinfo->sliceVec);
VSetAttr(VImageAttrList(res_image),"FOV",NULL,VStringRepn,xinfo->FOV);
/*--------*/
if(xinfo->fixpoint[0] != 'N')
VSetAttr(VImageAttrList(res_image), "fixpoint", NULL, VStringRepn, xinfo->fixpoint);
if(xinfo->ca[0] != 'N') {
VSetAttr(VImageAttrList(res_image), "ca", NULL, VStringRepn, xinfo->ca);
VSetAttr(VImageAttrList(res_image), "cp", NULL, VStringRepn, xinfo->cp);
VSetAttr(VImageAttrList(res_image), "extent", NULL, VStringRepn, xinfo->extent);
}
VAppendAttr(out_list, "image", NULL, VImageRepn, res_image);
for(i = 0; i < n; i++) {
beta_image[i] = VCreateImage(nslices, nrows, ncols, VFloatRepn);
VFillImage(beta_image[i], VAllBands, 0);
VSetAttr(VImageAttrList(beta_image[i]), "patient", NULL, VStringRepn, xinfo->patient);
VSetAttr(VImageAttrList(beta_image[i]), "voxel", NULL, VStringRepn, xinfo->voxel);
VSetAttr(VImageAttrList(beta_image[i]), "repetition_time", NULL, VLongRepn, itr);
VSetAttr(VImageAttrList(beta_image[i]), "talairach", NULL, VStringRepn, xinfo->talairach);
/* neu */
VSetAttr(VImageAttrList(beta_image[i]),"indexOrigin",NULL,VStringRepn,xinfo->indexOrigin);
VSetAttr(VImageAttrList(beta_image[i]),"columnVec",NULL,VStringRepn,xinfo->columnVec);
VSetAttr(VImageAttrList(beta_image[i]),"rowVec",NULL,VStringRepn,xinfo->rowVec);
VSetAttr(VImageAttrList(beta_image[i]),"sliceVec",NULL,VStringRepn,xinfo->sliceVec);
VSetAttr(VImageAttrList(beta_image[i]),"FOV",NULL,VStringRepn,xinfo->FOV);
/*--------*/
if(xinfo->fixpoint[0] != 'N')
VSetAttr(VImageAttrList(beta_image[i]), "fixpoint", NULL, VStringRepn, xinfo->fixpoint);
if(xinfo->ca[0] != 'N') {
VSetAttr(VImageAttrList(beta_image[i]), "ca", NULL, VStringRepn, xinfo->ca);
VSetAttr(VImageAttrList(beta_image[i]), "cp", NULL, VStringRepn, xinfo->cp);
VSetAttr(VImageAttrList(beta_image[i]), "extent", NULL, VStringRepn, xinfo->extent);
}
VSetAttr(VImageAttrList(beta_image[i]), "name", NULL, VStringRepn, "BETA");
VSetAttr(VImageAttrList(beta_image[i]), "modality", NULL, VStringRepn, "BETA");
VSetAttr(VImageAttrList(beta_image[i]), "beta", NULL, VShortRepn, i + 1);
VSetAttr(VImageAttrList(beta_image[i]), "df", NULL, VFloatRepn, df);
VAppendAttr(out_list, "image", NULL, VImageRepn, beta_image[i]);
}
VSetAttr(VImageAttrList(design), "name", NULL, VStringRepn, "X");
VSetAttr(VImageAttrList(design), "modality", NULL, VStringRepn, "X");
VAppendAttr(out_list, "image", NULL, VImageRepn, design);
KX_image = Mat2Vista(SX);
VSetAttr(VImageAttrList(KX_image), "name", NULL, VStringRepn, "KX");
VSetAttr(VImageAttrList(KX_image), "modality", NULL, VStringRepn, "KX");
VAppendAttr(out_list, "image", NULL, VImageRepn, KX_image);
VSetAttr(VImageAttrList(BCOV), "name", NULL, VStringRepn, "BCOV");
VSetAttr(VImageAttrList(BCOV), "modality", NULL, VStringRepn, "BCOV");
VAppendAttr(out_list, "image", NULL, VImageRepn, BCOV);
/*
** create temporary images for smoothness estimation
*/
/* smoothness estim only for 3D images, i.e. CA/CP known */
/* if(xinfo->ca[0] == 'N')
smooth = FALSE;*/
if(smooth) {
i0 = 20;
i1 = i0 + 30;
if(i1 > m)
i1 = m;
for(i = i0; i < i1; i++) {
if(i - i0 >= ETMP)
VError(" too many tmp images");
res_map[i - i0] = VCreateImage(nslices, nrows, ncols, VFloatRepn);
VFillImage(res_map[i - i0], VAllBands, 0);
}
}
/*
** process
*/
ys = gsl_vector_float_alloc(m);
y = gsl_vector_float_alloc(m);
z = gsl_vector_float_alloc(m);
beta = gsl_vector_float_alloc(n);
kernel = GaussKernel((double)sigma);
for(k = 0; k < nlists; k++) {
src[k] = VCreateImage(linfo[k].ntimesteps, nrows, ncols, linfo[k].repn);
VFillImage(src[k], VAllBands, 0);
}
npix = 0;
for(slice = 0; slice < nslices; slice++) {
if(slice % 5 == 0)
fprintf(stderr, " slice: %3d\r", slice);
for(k = 0; k < nlists; k++) {
if(linfo[k].zero[slice] == 0)
goto next1;
fd = open(linfo[k].filename, O_RDONLY);
if(fd == -1)
VError("could not open file %s", linfo[k].filename);
nt = linfo[k].ntimesteps;
if(! VReadBandDataFD(fd, &linfo[k].info[slice], 0, nt, &src[k]))
VError(" error reading data");
close(fd);
}
repn = linfo[0].repn;
for(row = 0; row < nrows; row++) {
for(col = 0; col < ncols; col++) {
for(k = 0; k < nlists; k++)
if(VPixel(src[k], 0, row, col, VShort) < minval + 1)
goto next;
npix++;
/* read time series data */
sum = sum2 = nx = 0;
ptr1 = y->data;
for(k = 0; k < nlists; k++) {
nt = VImageNBands(src[k]);
for(i = 0; i < nt; i++) {
u = VPixel(src[k], i, row, col, VShort);
(*ptr1++) = u;
sum += u;
sum2 += u * u;
nx++;
}
}
mean = sum / nx;
sig = sqrt((double)((sum2 - nx * mean * mean) / (nx - 1.0)));
if(sig < 0.001)
continue;
/* centering and scaling, Seber, p.330 */
ptr1 = y->data;
for(i = 0; i < m; i++) {
u = ((*ptr1) - mean) / sig;
(*ptr1++) = u + 100.0;
}
/* S x y */
ys = VectorConvolve(y, ys, kernel);
/* compute beta's */
fmat_x_vector(XInv, ys, beta);
/* residuals */
fmat_x_vector(SX, beta, z);
err = 0;
ptr1 = ys->data;
ptr2 = z->data;
for(i = 0; i < m; i++) {
d = ((*ptr1++) - (*ptr2++));
err += d * d;
}
/* sigma^2 */
var = err / trace;
/* write residuals output */
VPixel(res_image, slice, row, col, VFloat) = (VFloat)var;
/* save residuals of several timesteps for smoothness estimation */
if(smooth) {
ptr1 = ys->data;
ptr2 = z->data;
err = 0;
for(i = i0; i < i1; i++) {
d = ((*ptr1++) - (*ptr2++));
err += d * d;
VPixel(res_map[i - i0], slice, row, col, VFloat) = d;
}
if (err > 0) err = sqrt(err);
for(i = i0; i < i1; i++) {
d = VPixel(res_map[i - i0], slice, row, col, VFloat);
if (err > 1.0e-6) d /= err;
else d = 0;
VPixel(res_map[i - i0], slice, row, col, VFloat) = d / err;
}
}
/* write beta output */
ptr1 = beta->data;
for(i = 0; i < n; i++)
VPixel(beta_image[i], slice, row, col, VFloat) = (VFloat)(*ptr1++);
next:
;
}
}
next1:
;
}
/*
** Smoothness estimation based on residual images
*/
if(smooth) {
smooth_fwhm = VSmoothnessEstim(res_map, i1 - i0);
sscanf(xinfo->voxel, "%f %f %f", &vx, &vy, &vz);
vx = (vx + vy + vz) / 3.0; /* voxels should be isotropic */
smooth_fwhm *= vx;
fprintf(stderr, " smoothness: %f\n", smooth_fwhm);
VSetAttr(VImageAttrList(res_image), "smoothness", NULL, VFloatRepn, smooth_fwhm);
for(i = 0; i < n; i++) {
VSetAttr(VImageAttrList(beta_image[i]), "smoothness", NULL, VFloatRepn, smooth_fwhm);
}
for(i = 0; i < i1 - i0; i++)
VDestroyImage(res_map[i]);
}
ende:
if(npix == 0)
VError(" no voxels above threshold %d found", minval);
return out_list;
}
Matrix * Pose::rotation4D(const int axis,const float angle){
Matrix * rot = identity4D();
float sinAngle = sin(angle);
float cosAngle = cos(angle);
if(angle == 0.0){ //OPTIMIZAION POINT
return rot;
}
switch(axis){
case X_AXIS:
fmset(rot,Y_AXIS,Y_AXIS, cosAngle);
fmset(rot,Y_AXIS,Z_AXIS,-sinAngle);
fmset(rot,Z_AXIS,Y_AXIS, sinAngle);
fmset(rot,Z_AXIS,Z_AXIS, cosAngle);
break;
case Y_AXIS:
fmset(rot,X_AXIS,X_AXIS, cosAngle);
fmset(rot,X_AXIS,Z_AXIS, sinAngle);
fmset(rot,Z_AXIS,X_AXIS,-sinAngle);
fmset(rot,Z_AXIS,Z_AXIS, cosAngle);
break;
case Z_AXIS:
fmset(rot,X_AXIS,X_AXIS, cosAngle);
fmset(rot,X_AXIS,Y_AXIS,-sinAngle);
fmset(rot,Y_AXIS,X_AXIS, sinAngle);
fmset(rot,Y_AXIS,Y_AXIS, cosAngle);
break;
}
return rot;
}
inline LUDecomposition
makeLUD (const Matrix *A) {
LUDecomposition lud;
int m, n, pivsign, i, j, k, p;
float tmp;
lud.LU = fmatrix3(A);
m = A->rows;
n = A->cols;
lud.piv = (int*) malloc(m * sizeof(int));
i = 0;
while (i < m) {
lud.piv[i] = i;
i++;
}
pivsign = 1;
k = 0;
while (k < n) {
p = k;
i = k + 1;
while (i < m) {
if (fabs(*fmget(lud.LU,i,k)) > fabs(*fmget(lud.LU,p,k)))
p = i;
i++;
}
if (p != k) {
j = 0;
while (j < n) {
tmp = *fmget(lud.LU,p,j);
fmset(lud.LU, p, j, *fmget(lud.LU,k,j));
fmset(lud.LU, k, j, tmp);
j++;
}
i = lud.piv[p];
lud.piv[p] = lud.piv[k];
lud.piv[k] = i;
pivsign = -pivsign;
}
if (*fmget(lud.LU,k,k) != 0) {
i = k + 1;
while (i < m) {
fmset(lud.LU, i, k, *fmget(lud.LU,i,k) / *fmget(lud.LU,k,k));
j = k + 1;
while (j < n) {
fmset(lud.LU, i, j,
(*fmget(lud.LU,i,j) - *fmget(lud.LU,i,k) * *fmget(lud.LU,k,j))
);
j++;
}
i++;
}
}
k++;
}
return lud;
}
// Calculates a horizon line for real image via the camera matrix
void Pose::calcImageHorizonLine(Matrix * camera_to_world) {
//moving the camera frame to the center of the body
//lets us compare the rotation of the camera frame
//relative to the world frame
Matrix * camera_to_horizon = fmatrix3(camera_to_world);
fmset(camera_to_horizon,X_AXIS,W_AXIS,0.0f);
fmset(camera_to_horizon,Y_AXIS,W_AXIS,0.0f);
fmset(camera_to_horizon,Z_AXIS,W_AXIS,0.0f);
//optimization note: it is likely that the inverse in this case
//is just the transpose .. hmmm?
Matrix * horizon_to_camera = fminvert(camera_to_horizon);
vector<Matrix *> left_edge, right_edge; //need to be freed
left_edge.push_back(fmmul(camera_to_horizon,cameraEdgePoints[0]));
left_edge.push_back(fmmul(camera_to_horizon,cameraEdgePoints[1]));
right_edge.push_back(fmmul(camera_to_horizon,cameraEdgePoints[2]));
right_edge.push_back(fmmul(camera_to_horizon,cameraEdgePoints[3]));
// These are intersection points in the horizon plane.
Matrix * intersection_left = intersectLineWithXYPlane(&left_edge);
Matrix * intersection_right = intersectLineWithXYPlane(&right_edge);
// Now they are in the camera frame. Result still stored in intersection1,2
fmmuleq2(horizon_to_camera, intersection_left);
fmmuleq2(horizon_to_camera, intersection_right);
//we are only interested in the height (z axis), not the width
//width_mm = intersect.get(1,0)
//width_pix = -width_mm*MM_TO_PIX_X + IMAGE_WIDTH/2
const float height_mm_left = *fmget(intersection_left,2,0);
const float height_mm_right = *fmget(intersection_right,2,0);
const float height_pix_left = -height_mm_left*MM_TO_PIX_Y + IMAGE_HEIGHT/2;
const float height_pix_right = -height_mm_right*MM_TO_PIX_Y + IMAGE_HEIGHT/2;
fmfree(camera_to_horizon); fmfree(horizon_to_camera);
fmfree(left_edge[0]); fmfree(left_edge[1]);
fmfree(right_edge[0]); fmfree(right_edge[1]);
fmfree(intersection_left); fmfree(intersection_right);
horizon_left_2d.x = 0;
horizon_left_2d.y = static_cast<int>(height_pix_left);
horizon_right_2d.x = IMAGE_WIDTH - 1;
horizon_right_2d.y = static_cast<int>(height_pix_right);
/*
// calculates horizon left/right z's which are the z coordinates
// in body frame space if you project a 2d plane in the x,y axises
// at the focal point.
// subtract focal point z value from transformed point3 <float>'s z value.
float horizon_left_z = focal_point_body_frame.z-
image_matrix.transform(HORIZON_LEFT_3D).z;
// do same for right side
float horizon_right_z = focal_point_body_frame.z
-image_matrix.transform(HORIZON_RIGHT_3D).z;
// convert horizon_left_z and horizon_right_z to (x,y) coordinates in the
// image plane. converts the z mm value to a y pixel value.
horizon_left_2d.x = 0;
horizon_left_2d.y = (int)(-((horizon_left_z/MM_TO_PIX_Y_PLANE)-IMAGE_CENTER_Y));
horizon_right_2d.x = IMAGE_WIDTH-1;
horizon_right_2d.y = (int)(-((horizon_right_z/MM_TO_PIX_Y_PLANE)-IMAGE_CENTER_Y));
*/
// calculate slope of horizon + perpen
horizon_slope = (float)(horizon_right_2d.y - horizon_left_2d.y) / (float)(IMAGE_WIDTH-1);
if (horizon_slope != 0) {
perpen_slope = -1/horizon_slope;
}
else {
// need to include INFINITY macro here
}
}