void Compute_Eig_Dec_3D(int npixels, double *Dxx, double *Dxy, double *Dxz, double *Dyy, double *Dyz, double *Dzz,
double *Dvec1x, double *Dvec1y, double *Dvec1z, double *Dvec2x, double *Dvec2y, double *Dvec2z, double *Dvec3x, double *Dvec3y, double *Dvec3z,
double *Deiga, double *Deigb, double *Deigc)
{
double Ma[3][3];
double Davec[3][3];
double Daeig[3];
/* Loop variable */
int i;
for(i=0; i<npixels; i++)
{
Ma[0][0]=Dxx[i]; Ma[0][1]=Dxy[i]; Ma[0][2]=Dxz[i];
Ma[1][0]=Dxy[i]; Ma[1][1]=Dyy[i]; Ma[1][2]=Dyz[i];
Ma[2][0]=Dxz[i]; Ma[2][1]=Dyz[i]; Ma[2][2]=Dzz[i];
eigen_decomposition(Ma, Davec, Daeig);
Deiga[i]=Daeig[0]; Deigb[i]=Daeig[1]; Deigc[i]=Daeig[2];
/* Main direction (smallest eigenvector) */
Dvec1x[i] = Davec[0][0];
Dvec1y[i] = Davec[1][0];
Dvec1z[i] = Davec[2][0];
Dvec2x[i] = Davec[0][1];
Dvec2y[i] = Davec[1][1];
Dvec2z[i] = Davec[2][1];
Dvec3x[i] = Davec[0][2];
Dvec3y[i] = Davec[1][2];
Dvec3z[i] = Davec[2][2];
}
}
void mat3 :: mat3GetEigen ( float l [3], vec3 e [3] ) const
{
double a [3][3];
double d [3];
double v [3][3];
double * aPtr = (double *)a;
const float * mPtr = m;
for ( int i = 0; i < 9; i++ )
*aPtr++ = (double) *mPtr++;
eigen_decomposition ( a, v, d );
l [0] = (float) d [0];
l [1] = (float) d [1];
l [2] = (float) d [2];
e [0].x = (float) v [0][0];
e [0].y = (float) v [0][1];
e [0].z = (float) v [0][2];
e [1].x = (float) v [1][0];
e [1].y = (float) v [1][1];
e [1].z = (float) v [1][2];
e [2].x = (float) v [2][0];
e [2].y = (float) v [2][1];
e [2].z = (float) v [2][2];
}
int matrix_square_root_n(int n, double *X, double *I, double *Y) {
/*
This function calculates one of the square roots of the matrix X and stores it in Y:
X = sqrt(Y);
X needs to be a symmetric positive definite matrix of dimension n*n in Fortran vector
format. Y is of course a vector of similar size, and will contain the result on exit.
Y will be used as a workspace variable in the meantime.
The variable I is a vector of length n containing +1 and -1 elements. It can be used
to select one of the 2^n different square roots of X
This function first calculates the eigenvalue decomposition of X: X = U*D*U^T
A new matrix F is then calculated with on the diagonal the square roots of D, with
signs taken from I.
The square root is then obtained by calculating U*F*U^T
*/
if (check_input(X, Y, "matrix_square_root_n")) return 1;
double *eigval, *eigvec, *temp;
int info = 0, i, j;
/* Memory allocation */
eigval=(double*) malloc(n*sizeof(double));
eigvec=(double*) malloc(n*n*sizeof(double));
temp=(double*) malloc(n*n*sizeof(double));
if ((eigval==NULL)||(eigvec==NULL)||(temp==NULL)) {
printf("malloc failed in matrix_square_root_n\n");
return 2;
}
/* Eigen decomposition */
info=eigen_decomposition(n, X, eigvec, eigval);
if (info != 0) return info;
/* Check for positive definitiveness*/
for (i=0; i<n; i++) if (eigval[i]<0) {
fprintf(stderr, "In matrix_square_root_n: Matrix is not positive definite.\n");
return 1;
}
/* Build square rooted diagonal matrix, with sign signature I */
for (i=0; i<n; i++) for (j=0; j<n; j++) Y[i*n+j] = 0.0;
for (i=0; i<n; i++) Y[i*n+i] = I[i]*sqrt(eigval[i]);
/* Multiply the eigenvectors with this diagonal matrix Y. Store back in Y */
matrix_matrix_mult(n, n, n, 1.0, 0, eigvec, Y, temp);
vector_copy(n*n, temp, Y);
/* Transpose eigenvectors. Store in temp */
matrix_transpose(n, n, eigvec, temp);
/* Multiply Y with this temp. Store in eigvec which is no longer required. Copy to Y */
matrix_matrix_mult(n, n, n, 1.0, 0, Y, temp, eigvec);
vector_copy(n*n, eigvec, Y);
/* Cleanup and exit */
free(eigvec); free(eigval); free(temp);
return info;
}
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
void pf_matrix_unitary (pf_matrix_t* r, pf_matrix_t* d, pf_matrix_t a)
{
int i, j;
/*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace *w;
aa = gsl_matrix_alloc(3, 3);
eval = gsl_vector_alloc(3);
evec = gsl_matrix_alloc(3, 3);
*/
double aa[3][3];
double eval[3];
double evec[3][3];
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
//gsl_matrix_set(aa, i, j, a.m[i][j]);
aa[i][j] = a.m[i][j];
}
}
// Compute eigenvectors/values
/*
w = gsl_eigen_symmv_alloc(3);
gsl_eigen_symmv(aa, eval, evec, w);
gsl_eigen_symmv_free(w);
*/
eigen_decomposition (aa, evec, eval);
*d = pf_matrix_zero();
for (i = 0; i < 3; i++)
{
//d->m[i][i] = gsl_vector_get(eval, i);
d->m[i][i] = eval[i];
for (j = 0; j < 3; j++)
{
//r->m[i][j] = gsl_matrix_get(evec, i, j);
r->m[i][j] = evec[i][j];
}
}
//gsl_matrix_free(evec);
//gsl_vector_free(eval);
//gsl_matrix_free(aa);
return;
}
// This routine was written by Jernej Barbic.
void eigen_sym(Mat3d & a, Vec3d & eig_val, Vec3d eig_vec[3])
{
double A[3][3] = { {a[0][0], a[0][1], a[0][2]},
{a[1][0], a[1][1], a[1][2]},
{a[2][0], a[2][1], a[2][2]} };
double V[3][3];
double d[3];
eigen_decomposition(A, V, d);
eig_val = Vec3d(d[2],d[1],d[0]);
if(eig_vec)
{
eig_vec[0] = Vec3d(V[0][2], V[1][2], V[2][2]);
eig_vec[1] = Vec3d(V[0][1], V[1][1], V[2][1]);
eig_vec[2] = Vec3d(V[0][0], V[1][0], V[2][0]);
}
}
int sqrtm(const dgematrix &A, dgematrix & sqrtA)
{
dgematrix P,D;
int i;
if (!eigen_decomposition(A,P,D))
{
for (i=0;i<D.m; i++)
D(i,i)=sqrt(fabs(D(i,i)));
sqrtA = P * D * t(P);
return 0;
}
else
{
cerr<<"BFilt :: SQRTM Problem."<<endl;
return 1;
}
}
// Add Tompson
void getSymMatEigenvectors(double3 * ret, const double3x3 &A)
{
double inputA[3][3];
double outputEigVec[3][3];
double outputEigVal[3];
inputA[0][0] = A[0][0]; inputA[0][1] = A[0][1]; inputA[0][2] = A[0][2];
inputA[1][0] = A[1][0]; inputA[1][1] = A[1][1]; inputA[1][2] = A[1][2];
inputA[2][0] = A[2][0]; inputA[2][1] = A[2][1]; inputA[2][2] = A[2][2];
eigen_decomposition(inputA, outputEigVec, outputEigVal);
// The eigen vectors are colonm-wise in the return vector --> Verified with matlab
// Transforming <1,0,0> with the return matrix gives us the column of the origional matrix (which suggests I am correct).
ret[0].x = outputEigVec[0][0]; ret[0].y = outputEigVec[1][0]; ret[0].z = outputEigVec[2][0];
ret[1].x = outputEigVec[0][1]; ret[1].y = outputEigVec[1][1]; ret[1].z = outputEigVec[2][1];
ret[2].x = outputEigVec[0][2]; ret[2].y = outputEigVec[1][2]; ret[2].z = outputEigVec[2][2];
}
int sqrtm(const dsymatrix &A, dgematrix & sqrtA)
{
dgematrix P,D;
int i;
if (!cholesky(A,sqrtA))
{
return 0;
}
if (!eigen_decomposition(A,P,D))
{
for (i=0;i<D.m; i++)
{
D(i,i)=sqrt(fabs(D(i,i)));
}
sqrtA = P * D * t(P);
return 0;
}
cerr<<"BFilt :: SQRTM Problem."<<endl;
return 1;
}
void GaussianDistribution::updateParameters(glm::vec3 m, glm::mat3 c) {
_mean = m;
_cov = c;
det = glm::determinant(_cov);
firstTerm = glm::sqrt(glm::pow(2 * 3.14159265359f, 3.0f) * det);
inv = glm::inverse(_cov);
double A[3][3];
double V[3][3];
double d[3];
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
A[i][j] = _cov[i][j];
eigen_decomposition(A, V, d);
_U = glm::mat3(V[0][0], V[0][1], V[0][2], V[1][0], V[1][1], V[1][2], V[2][0], V[2][1], V[2][2]);
_sqrtsig = glm::mat3(sqrt(d[0]), 0, 0, 0, sqrt(d[1]), 0, 0, 0, sqrt(d[2]));
// mozda bitno!
_U = glm::transpose(_U);
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
double *Dxx, *Dxy, *Dxz, *Dyy, *Dyz, *Dzz;
double *Dvecx, *Dvecy, *Dvecz, *Deiga, *Deigb, *Deigc;
float *Dxx_f, *Dxy_f, *Dxz_f, *Dyy_f, *Dyz_f, *Dzz_f;
float *Dvecx_f, *Dvecy_f, *Dvecz_f, *Deiga_f, *Deigb_f, *Deigc_f;
mwSize output_dims[2]={1, 3};
double Ma[3][3];
double Davec[3][3];
double Daeig[3];
/* Loop variable */
int i;
/* Size of input */
const mwSize *idims;
int nsubs=0;
/* Number of pixels */
int npixels=1;
/* Check for proper number of arguments. */
if(nrhs!=6) {
mexErrMsgTxt("Six inputs are required.");
} else if(nlhs<3) {
mexErrMsgTxt("Three or Six outputs are required");
}
/* Get the number of dimensions */
nsubs = mxGetNumberOfDimensions(prhs[0]);
/* Get the sizes of the inputs */
idims = mxGetDimensions(prhs[0]);
for (i=0; i<nsubs; i++) { npixels=npixels*idims[i]; }
if(mxGetClassID(prhs[0])==mxDOUBLE_CLASS) {
/* Assign pointers to each input. */
Dxx = (double *)mxGetPr(prhs[0]);
Dxy = (double *)mxGetPr(prhs[1]);
Dxz = (double *)mxGetPr(prhs[2]);
Dyy = (double *)mxGetPr(prhs[3]);
Dyz = (double *)mxGetPr(prhs[4]);
Dzz = (double *)mxGetPr(prhs[5]);
/* Assign pointers to each output. */
plhs[0] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
plhs[1] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
plhs[2] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
Deiga = mxGetPr(plhs[0]); Deigb = mxGetPr(plhs[1]); Deigc = mxGetPr(plhs[2]);
if(nlhs==6) {
/* Main direction (larged eigenvector) */
plhs[3] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
plhs[4] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
plhs[5] = mxCreateNumericArray(nsubs, idims, mxDOUBLE_CLASS, mxREAL);
Dvecx = mxGetPr(plhs[3]); Dvecy = mxGetPr(plhs[4]); Dvecz = mxGetPr(plhs[5]);
}
for(i=0; i<npixels; i++) {
Ma[0][0]=Dxx[i]; Ma[0][1]=Dxy[i]; Ma[0][2]=Dxz[i];
Ma[1][0]=Dxy[i]; Ma[1][1]=Dyy[i]; Ma[1][2]=Dyz[i];
Ma[2][0]=Dxz[i]; Ma[2][1]=Dyz[i]; Ma[2][2]=Dzz[i];
eigen_decomposition(Ma, Davec, Daeig);
Deiga[i]=Daeig[0]; Deigb[i]=Daeig[1]; Deigc[i]=Daeig[2];
if(nlhs==6) {
/* Main direction (smallest eigenvector) */
Dvecx[i]=Davec[0][0];
Dvecy[i]=Davec[1][0];
Dvecz[i]=Davec[2][0];
}
}
}
else if(mxGetClassID(prhs[0])==mxSINGLE_CLASS) {
/* Assign pointers to each input. */
Dxx_f = (float *)mxGetPr(prhs[0]);
Dxy_f = (float *)mxGetPr(prhs[1]);
Dxz_f = (float *)mxGetPr(prhs[2]);
Dyy_f = (float *)mxGetPr(prhs[3]);
Dyz_f = (float *)mxGetPr(prhs[4]);
Dzz_f = (float *)mxGetPr(prhs[5]);
/* Assign pointers to each output. */
plhs[0] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[1] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[2] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
Deiga_f = (float *)mxGetPr(plhs[0]);
Deigb_f = (float *)mxGetPr(plhs[1]);
Deigc_f = (float *)mxGetPr(plhs[2]);
if(nlhs==6) {
/* Main direction (smallest eigenvector) */
plhs[3] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[4] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[5] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
Dvecx_f = (float *)mxGetPr(plhs[3]);
Dvecy_f = (float *)mxGetPr(plhs[4]);
Dvecz_f = (float *)mxGetPr(plhs[5]);
}
for(i=0; i<npixels; i++) {
Ma[0][0]=(double)Dxx_f[i]; Ma[0][1]=(double)Dxy_f[i]; Ma[0][2]=(double)Dxz_f[i];
Ma[1][0]=(double)Dxy_f[i]; Ma[1][1]=(double)Dyy_f[i]; Ma[1][2]=(double)Dyz_f[i];
Ma[2][0]=(double)Dxz_f[i]; Ma[2][1]=(double)Dyz_f[i]; Ma[2][2]=(double)Dzz_f[i];
eigen_decomposition(Ma, Davec, Daeig);
Deiga_f[i]=(float)Daeig[0];
Deigb_f[i]=(float)Daeig[1];
Deigc_f[i]=(float)Daeig[2];
if(nlhs==6) {
/* Main direction (smallest eigenvector) */
Dvecx_f[i]=(float)Davec[0][0];
Dvecy_f[i]=(float)Davec[1][0];
Dvecz_f[i]=(float)Davec[2][0];
}
}
}
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
float *Dxx, *Dxy, *Dxz, *Dyy, *Dyz, *Dzz;
float *Jxx, *Jxy, *Jxz, *Jyy, *Jyz, *Jzz;
/* Matrices of Eigenvector calculation */
double Ma[3][3];
double Davec[3][3];
double Daeig[3];
/* Eigenvector and eigenvalues as scalars */
double mu1, mu2, mu3, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z;
/* Amplitudes of diffustion tensor */
double lambda1, lambda2, lambda3;
/* Constants used in diffusion tensor calculation */
double alpha, C, m;
/* Denominator */
double di;
/* Eps for finite values */
float eps=(float)1e-20;
/* Used to read Options structure */
double *OptionsField;
int field_num;
/* Loop variable */
int i;
/* Size of input */
const mwSize *idims;
int nsubs=0;
/* Number of pixels */
int npixels=1;
/* Check for proper number of arguments. */
if(nrhs!=7) {
mexErrMsgTxt("Seven inputs are required.");
} else if(nlhs!=6) {
mexErrMsgTxt("Six outputs are required");
}
for(i=0; i<6; i++) {
if(!mxIsSingle(prhs[i])){ mexErrMsgTxt("Inputs must be single"); }
}
/* Get constants from Options structure */
if(!mxIsStruct(prhs[6])){ mexErrMsgTxt("Options must be structure"); }
field_num = mxGetFieldNumber(prhs[6], "alpha");
if(field_num>=0) {
OptionsField=mxGetPr(mxGetFieldByNumber(prhs[6], 0, field_num));
alpha=(double)OptionsField[0];
}
field_num = mxGetFieldNumber(prhs[6], "C");
if(field_num>=0) {
OptionsField=mxGetPr(mxGetFieldByNumber(prhs[6], 0, field_num));
C=(double)OptionsField[0];
}
field_num = mxGetFieldNumber(prhs[6], "m");
if(field_num>=0) {
OptionsField=mxGetPr(mxGetFieldByNumber(prhs[6], 0, field_num));
m=(double)OptionsField[0];
}
/* Get the number of dimensions */
nsubs = mxGetNumberOfDimensions(prhs[0]);
/* Get the sizes of the inputs */
idims = mxGetDimensions(prhs[0]);
for (i=0; i<nsubs; i++) { npixels=npixels*idims[i]; }
/* Create output Tensor Volumes */
plhs[0] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[1] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[2] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[3] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[4] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
plhs[5] = mxCreateNumericArray(nsubs, idims, mxSINGLE_CLASS, mxREAL);
/* Assign pointers to each input. */
Jxx = (float *)mxGetPr(prhs[0]);
Jxy = (float *)mxGetPr(prhs[1]);
Jxz = (float *)mxGetPr(prhs[2]);
Jyy = (float *)mxGetPr(prhs[3]);
Jyz = (float *)mxGetPr(prhs[4]);
Jzz = (float *)mxGetPr(prhs[5]);
/* Assign pointers to each output. */
Dxx = (float *)mxGetPr(plhs[0]);
Dxy = (float *)mxGetPr(plhs[1]);
Dxz = (float *)mxGetPr(plhs[2]);
Dyy = (float *)mxGetPr(plhs[3]);
Dyz = (float *)mxGetPr(plhs[4]);
Dzz = (float *)mxGetPr(plhs[5]);
for(i=0; i<npixels; i++) {
/* Calculate eigenvectors and values of local Hessian */
Ma[0][0]=(double)Jxx[i]+eps; Ma[0][1]=(double)Jxy[i]; Ma[0][2]=(double)Jxz[i];
Ma[1][0]=(double)Jxy[i]; Ma[1][1]=(double)Jyy[i]+eps; Ma[1][2]=(double)Jyz[i];
Ma[2][0]=(double)Jxz[i]; Ma[2][1]=(double)Jyz[i]; Ma[2][2]=(double)Jzz[i]+eps;
eigen_decomposition(Ma, Davec, Daeig);
/* Convert eigenvector and eigenvalue matrices back to scalar variables */
mu1=Daeig[2];
mu2=Daeig[1];
mu3=Daeig[0];
v1x=Davec[0][0]; v1y=Davec[1][0]; v1z=Davec[2][0];
v2x=Davec[0][1]; v2y=Davec[1][1]; v2z=Davec[2][1];
v3x=Davec[0][2]; v3y=Davec[1][2]; v3z=Davec[2][2];
/* Scaling of diffusion tensor */
di=mu1-mu3;
if((di<eps)&&(di>-eps)) { lambda1 = alpha; } else { lambda1 = alpha + (1.0- alpha)*exp(-C/pow(di,(2.0*m))); }
lambda2 = alpha;
lambda3 = alpha;
/* Construct the diffusion tensor */
Dxx[i] = (float)(lambda1*v1x*v1x + lambda2*v2x*v2x + lambda3*v3x*v3x);
Dyy[i] = (float)(lambda1*v1y*v1y + lambda2*v2y*v2y + lambda3*v3y*v3y);
Dzz[i] = (float)(lambda1*v1z*v1z + lambda2*v2z*v2z + lambda3*v3z*v3z);
Dxy[i] = (float)(lambda1*v1x*v1y + lambda2*v2x*v2y + lambda3*v3x*v3y);
Dxz[i] = (float)(lambda1*v1x*v1z + lambda2*v2x*v2z + lambda3*v3x*v3z);
Dyz[i] = (float)(lambda1*v1y*v1z + lambda2*v2y*v2z + lambda3*v3y*v3z);
}
}
void updateHandPosition(PointCloud handPcl) {
int i, j;
int pointCount = barycenter(handPcl, &barycX, &barycY, &barycZ) ;
if (pointCount < MIN_HAND_POINT_NUMBER) {
return;
}
isOpenHand = openHand(handPcl);
double posCovarMat[3][3];
double posCovarMatEigVec[3][3];
double posCovarMatEigVal[3];
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
posCovarMat[i][j] = 0;
}
}
for(j=0; j<FREENECT_FRAME_H; j++) {
for(i=0; i<FREENECT_FRAME_W; i++) {
if (handPcl[j][i].valid) {
posCovarMat[0][0] += (handPcl[j][i].x - barycX)*(handPcl[j][i].x - barycX);
posCovarMat[0][1] += (handPcl[j][i].x - barycX)*(handPcl[j][i].y - barycY);
posCovarMat[0][2] += (handPcl[j][i].x - barycX)*(handPcl[j][i].z - barycZ);
posCovarMat[1][0] += (handPcl[j][i].y - barycY)*(handPcl[j][i].x - barycX);
posCovarMat[1][1] += (handPcl[j][i].y - barycY)*(handPcl[j][i].y - barycY);
posCovarMat[1][2] += (handPcl[j][i].y - barycY)*(handPcl[j][i].z - barycZ);
posCovarMat[2][0] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].x - barycX);
posCovarMat[2][1] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].y - barycY);
posCovarMat[2][2] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].z - barycZ);
}
}
}
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
posCovarMat[i][j] = posCovarMat[i][j] / pointCount;
}
}
eigen_decomposition(posCovarMat, posCovarMatEigVec, posCovarMatEigVal);
// printMatrix("Covar Mat", posCovarMat, 3);
// printMatrix("Pos Eigen Vec", posCovarMatEigVec, 3);
majorAxeX = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[0][2] : - posCovarMatEigVec[0][2];
majorAxeY = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[1][2] : - posCovarMatEigVec[1][2];
majorAxeZ = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[2][2] : - posCovarMatEigVec[2][2];
normVecX = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[0][0] : - posCovarMatEigVec[0][0];
normVecY = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[1][0] : - posCovarMatEigVec[1][0];
normVecZ = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[2][0] : - posCovarMatEigVec[2][0];
//arduinoCmd();
//printf("Barycenter: X: %.4f Y: %.4f Z: %.4f\tDirection: X: %.4f Y: %.4f Z: %.4f\n", barycX, barycY, barycZ, normVecX, normVecY, normVecZ);
moveMouse();
}
unsigned __stdcall StructureTensor2DiffusionTensorThread(float **Args) {
#else
void StructureTensor2DiffusionTensorThread(float **Args) {
#endif
/* Matrices of Eigenvector calculation */
double Ma[3][3];
double Davec[3][3];
double Daeig[3];
/* Eigenvector and eigenvalues as scalars */
double mu1, mu2, mu3, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z;
/* Magnitude of gradient */
float *gradA;
/* Amplitudes of diffustion tensor */
double lambda1, lambda2, lambda3;
double lambdac1, lambdac2, lambdac3;
double lambdae1, lambdae2, lambdae3;
/* Eps for finite values */
const float eps=(float)1e-20;
/* Loop variable */
int i;
/* Number of pixels */
int npixels=1;
/* The diffusion tensors and structure tensors */
float *Jxx, *Jxy, *Jxz, *Jyy, *Jyz, *Jzz;
float *Dxx, *Dxy, *Dxz, *Dyy, *Dyz, *Dzz;
int dimsu[3];
float *dimsu_f, *constants_f, *Nthreads_f, *ThreadID_f;
/* Number of threads */
int ThreadOffset, Nthreads;
/* Constants */
double C, m, alpha, lambda_h, lambda_e, lambda_c;
/* Choice of eigenvalue equation */
int eigenmode;
/* Temporary variables */
double di, epsilon, xi;
Jxx=Args[0];
Jxy=Args[1];
Jxz=Args[2];
Jyy=Args[3];
Jyz=Args[4];
Jzz=Args[5];
Dxx=Args[6];
Dxy=Args[7];
Dxz=Args[8];
Dyy=Args[9];
Dyz=Args[10];
Dzz=Args[11];
gradA=Args[12];
dimsu_f=Args[13];
constants_f=Args[14];
ThreadID_f=Args[15];
Nthreads_f=Args[16];
for(i=0;i<3;i++){ dimsu[i]=(int)dimsu_f[i]; }
eigenmode=(int)constants_f[0];
C=(double)constants_f[1];
m=(double)constants_f[2];
alpha=(double)constants_f[3];
lambda_e=(double)constants_f[4];
lambda_h=(double)constants_f[5];
lambda_c=(double)constants_f[6];
ThreadOffset=(int)ThreadID_f[0];
Nthreads=(int)Nthreads_f[0];
npixels=dimsu[0]*dimsu[1]*dimsu[2];
for(i=ThreadOffset; i<npixels; i=i+Nthreads) {
/* Calculate eigenvectors and values of local Hessian */
Ma[0][0]=(double)Jxx[i]+eps; Ma[0][1]=(double)Jxy[i]; Ma[0][2]=(double)Jxz[i];
Ma[1][0]=(double)Jxy[i]; Ma[1][1]=(double)Jyy[i]+eps; Ma[1][2]=(double)Jyz[i];
Ma[2][0]=(double)Jxz[i]; Ma[2][1]=(double)Jyz[i]; Ma[2][2]=(double)Jzz[i]+eps;
eigen_decomposition(Ma, Davec, Daeig);
/* Convert eigenvector and eigenvalue matrices back to scalar variables */
mu1=Daeig[2];
mu2=Daeig[1];
mu3=Daeig[0];
v1x=Davec[0][0]; v1y=Davec[1][0]; v1z=Davec[2][0];
v2x=Davec[0][1]; v2y=Davec[1][1]; v2z=Davec[2][1];
v3x=Davec[0][2]; v3y=Davec[1][2]; v3z=Davec[2][2];
/* Scaling of diffusion tensor */
if(eigenmode==0) /* Weickert line shaped */
{
di=(mu1-mu3);
if((di<eps)&&(di>-eps)) { lambda1 = alpha; } else { lambda1 = alpha + (1.0- alpha)*exp(-C/pow(di,(2.0*m))); }
lambda2 = alpha;
lambda3 = alpha;
}
else if(eigenmode==1) /* Weickert plane shaped */
{
di=(mu1-mu3);
if((di<eps)&&(di>-eps)) { lambda1 = alpha; } else { lambda1 = alpha + (1.0- alpha)*exp(-C/pow(di,(2.0*m))); }
di=(mu2-mu3);
if((di<eps)&&(di>-eps)) { lambda2 = alpha; } else { lambda2 = alpha + (1.0- alpha)*exp(-C/pow(di,(2.0*m))); }
lambda3 = alpha;
}
else if (eigenmode==2) /* EED */
{
if(gradA[i]<eps){ lambda3 = 1; } else { lambda3 = 1 - exp(-3.31488/pow4(gradA[i]/pow2(lambda_e))); }
lambda2 = 1;
lambda1 = 1;
}
else if (eigenmode==3) /* CED */
{
lambda3 = alpha;
lambda2 = alpha;
if((mu2<eps)&&(mu2>-eps)){ lambda1=1;}
else if ((mu3<eps)&&(mu3>-eps)) { lambda1=1; }
else { lambda1= alpha + (1.0- alpha)*exp(-0.6931*pow2(lambda_c)/pow4(mu2/(alpha+mu3))); }
}
else if (eigenmode==4) /* Hybrid Diffusion with Continous Switch */
{
if(gradA[i]<eps) { lambdae3 = 1; } else { lambdae3 = 1 - exp(-3.31488/pow4(gradA[i]/pow2(lambda_e))); }
lambdae2 = 1;
lambdae1 = 1;
lambdac3 = alpha;
lambdac2 = alpha;
if((mu2<eps)&&(mu2>-eps)){ lambdac1=1;}
else if ((mu3<eps)&&(mu3>-eps)) { lambdac1=1;}
else { lambdac1 = alpha + (1.0- alpha)*exp(-0.6931*pow2(lambda_c)/pow4(mu2/(alpha+mu3))); }
xi= ( (mu1 / (alpha + mu2)) - (mu2 / (alpha + mu3) ));
di=2.0*pow4(lambda_h);
epsilon = exp(mu2*(pow2(lambda_h)*(xi-absd(xi))-2.0*mu3)/di);
lambda1 = (1-epsilon) * lambdac1 + epsilon * lambdae1;
lambda2 = (1-epsilon) * lambdac2 + epsilon * lambdae2;
lambda3 = (1-epsilon) * lambdac3 + epsilon * lambdae3;
}
/* Construct the diffusion tensor */
Dxx[i] = (float)(lambda1*v1x*v1x + lambda2*v2x*v2x + lambda3*v3x*v3x);
Dyy[i] = (float)(lambda1*v1y*v1y + lambda2*v2y*v2y + lambda3*v3y*v3y);
Dzz[i] = (float)(lambda1*v1z*v1z + lambda2*v2z*v2z + lambda3*v3z*v3z);
Dxy[i] = (float)(lambda1*v1x*v1y + lambda2*v2x*v2y + lambda3*v3x*v3y);
Dxz[i] = (float)(lambda1*v1x*v1z + lambda2*v2x*v2z + lambda3*v3x*v3z);
Dyz[i] = (float)(lambda1*v1y*v1z + lambda2*v2y*v2z + lambda3*v3y*v3z);
}
/* explicit end thread, helps to ensure proper recovery of resources allocated for the thread */
#ifdef _WIN32
_endthreadex( 0 );
return 0;
#else
pthread_exit(NULL);
#endif
}
