void myMxM(Matrix A, Matrix v, Matrix u)
{
Matrix temp = createMatrix(A->rows, v->cols);
#pragma omp parallel
{
int* displ, *cols;
splitVector(v->cols, num_threads(), &cols, &displ);
MxM2(A, v, temp, displ[get_thread()], cols[get_thread()],
displ[get_thread()], 1.0, 0.0);
free(cols);
free(displ);
}
#ifdef HAVE_MPI
for (int i=0;i<v->as_vec->comm_size;++i) {
Matrix t = subMatrix(temp, v->as_vec->displ[i]/v->cols,
v->as_vec->sizes[i]/v->cols, 0, v->cols);
MPI_Reduce(t->data[0], u->data[0], v->as_vec->sizes[i],
MPI_DOUBLE, MPI_SUM, i, *v->as_vec->comm);
freeMatrix(t);
}
#else
memcpy(u->data[0], temp->data[0], u->as_vec->len*sizeof(double));
#endif
freeMatrix(temp);
}
/**
* @brief Overrides - operator for IntMatrix to subtract a matrix from the current matrix.
* @param IntMatrix we wish to subtract from the current matrix.
* @return a new IntMatrix we created
*/
const IntMatrix IntMatrix :: operator-(const IntMatrix &other) const
{
// copy our matrix, use -= to subtract other from it, then return it
IntMatrix subMatrix(*this);
subMatrix -= other;
return subMatrix;
}
void
gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt)
{
float D = elem(sizeOfMatrix(*m_opt),0,1)+1; //printf("%f\n", D);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = elem(sizeOfMatrix(*xEst),0,0); //printf("%f\n", Nx);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
/* Eigenvectors, Eigenvalues */
int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 );
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val );
/* m1 = vec*sqrtf(val) */
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val);
freeMatrix(Vec);
freeMatrix(Val);
/* rotate & scale samples: m = m1*S */
Matrix m = scaledSamplePoints(m1, *m_opt);
/* x = x*ones(1,d) */
Matrix x = fillMatrix(*xEst, d);
/* shift samples: m = m + x */
m = addMatrix(m, x); //printMatrix(m);
/* afun */
/* Prediction: mean
xPredDiracs = feval(afun,m, [], [], t);
xPred = w*sum(xPredDiracs, 2);*/
Matrix xPredDiracs = (*afun) (m, *dt); //printMatrix(xPredDiracs);
Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2); //printMatrix(xPredDiracsSum);
Matrix xPred = mulScalarMatrix(w, xPredDiracsSum); //printMatrix(xPred);
//mxDiracs = xPredDiracs-repmat(xPred, 1, d);
//CPred = w_opt*mxDiracs*mxDiracs';
Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d)); //printMatrix(mxDiracs);
Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs))); //printMatrix(CPred);
//RETURN
*xEst = xPred; //printMatrix(*xEst);
*CEst = CPred; //printMatrix(*CEst);
freeMatrix(m);
freeMatrix(xPredDiracs);
freeMatrix(xPredDiracsSum);
}
int gcm::patchRun(vector<SLR_ST_Skeleton> vSkeletonData, vector<Mat> vDepthData, vector<IplImage*> vColorData,
int *rankIndex, double *rankScore)
{
int kernelFeatureDim = NClass*NTrainSample;
//clock_t startT=clock();
oriData2Feature(vSkeletonData, vDepthData, vColorData);
//cout<<"=======Time========="<<clock()-startT<<endl;
gcmSubspace();
x[0].index = 0;
for (int j=0; j<kernelFeatureDim; j++)
{
subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim);
x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim);
x[j+1].index=j+1;
}
x[kernelFeatureDim+1].index=-1;
//int testID = svm_predict_probability(myModel, x, prob_estimates);
int testID_noPro = svm_predict(myModel, x);
int testID = svm_predict_probability(myModel_candi, x, prob_estimates);
//Sort and get the former 5 ranks.
vector<scoreAndIndex> rank;
for (int i=0; i<myModel->nr_class; i++)
{
scoreAndIndex temp;
temp.index = myModel->label[i];
temp.score = prob_estimates[i];
rank.push_back(temp);
}
sort(rank.begin(),rank.end(),comp);
rankIndex[0] = testID_noPro;
rankScore[0] = 1.0;
int candiN = 0;
//for (int i=1; i<5; i++)
int seqCandiN = 1;
while(seqCandiN<5)
{
if (rank[candiN].index == testID_noPro)
{
candiN++;
continue;
}
rankIndex[seqCandiN] = rank[candiN].index;
rankScore[seqCandiN] = rank[candiN].score;
candiN++;
seqCandiN++;
}
releaseResource();
return rankIndex[0];
}
matrix matrix::getRows(int nNum, const int c[])
{
matrix subMatrix(nNum, col);
for(int i=0; i<nNum; i++)
{
for(int j=0; j<col; j++)
{
subMatrix.array[i][j] = array[c[i]][j];
}
}
return subMatrix;
}
matrix matrix::getColumns(int nNum, const int c[])
{
matrix subMatrix(row, nNum);
for(int i=0; i<row; i++)
{
for(int j=0; j<nNum; j++)
{
subMatrix.array[i][j] = array[i][c[j]];
}
}
return subMatrix;
}
void kalmanProcess(kalman_s *mykalman)
{
int i,j;
for(i=0;i<NM;i++)
memset(mykalman->temp_4_4[i],0,sizeof(double)*NM);
//第一个公式：x(k|k-1) = Ax(k-1|k-1)
multiplyMatrix(mykalman->A,NS,NS,mykalman->X,NS,1,mykalman->temp_1); //X=A*X
for (i=0;i<NS;i++)
mykalman->X[i][0]=mykalman->temp_1[i][0];
//第二个公式： P = A*P*A'+Q
multiplyMatrix(mykalman->A,NS,NS,mykalman->P,NS,NS,mykalman->temp_2_1); //temp_2_1 = A*P
transpositionMatrix(mykalman->A, mykalman->temp_2, NS, NS); //temp_2 = A'
multiplyMatrix(mykalman->temp_2_1,NS,NS,mykalman->temp_2,NS,NS,mykalman->P); //P = A*P*A’
addMatrix(mykalman->P,NS,NS,mykalman->Q,NS,NS,mykalman->P); //P = A*P*A’+Q
//第三个公式： X = X+K*[Z-H*X]
multiplyMatrix(mykalman->H,NM,NS,mykalman->X,NS,1,mykalman->temp_3_1); //temp_3_1=H*X
subMatrix(mykalman->Z,NM,1,mykalman->temp_3_1,NM,1,mykalman->temp_3_1); //temp_3_1=Z-H*X
multiplyMatrix(mykalman->K,NS,NM,mykalman->temp_3_1,NM,1,mykalman->temp_3_2); //temp_3_2 = K*(Z-H*X)
addMatrix(mykalman->X,NS,1,mykalman->temp_3_2,NS,1,mykalman->X); //X = X+ K*(Z-H*X)
//第四个公式：K = P*H'/[H*P*H'+R]
transpositionMatrix(mykalman->H,mykalman->temp_4_3,NM,NS); //temp_4_3 = H'
multiplyMatrix(mykalman->P,NS,NS,mykalman->temp_4_3,NS,NM,mykalman->temp_4_1); //temp_4_1 = P*H'
multiplyMatrix(mykalman->H,NM,NS,mykalman->temp_4_1,NS,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'
addMatrix(mykalman->temp_4_2,NM,NM,mykalman->R,NM,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'+R
InverseMatrix(mykalman->temp_4_2, mykalman->temp_4_4, NM,NM); //temp_4_4=~(H*P*H'+R)
multiplyMatrix(mykalman->temp_4_1,NS,NM,mykalman->temp_4_4,NM,NM,mykalman->K); //K = P*H'*~(H*P*H'+R)
//第五个公式：P = [I-K*H]*P
multiplyMatrix(mykalman->K,NS,NM,mykalman->H,NM,NS,mykalman->temp_5); //temp_5 = K*H
subMatrix(mykalman->E,NS,NS,mykalman->temp_5,NS,NS,mykalman->temp_5_1); //temp_5_1 = E-K*H
multiplyMatrix(mykalman->temp_5_1,NS,NS,mykalman->P,NS,NS,mykalman->temp_5_2); //temp_5_2 = (E-K*H)*P
for (i=0;i<NS;i++)
for (j=0;j<NS;j++)
mykalman->P[i][j] = mykalman->temp_5_2[i][j];
}
//==========================================================================
// Class: Matrix
// Function: GetSubMatrix
//
// Description: Returns a sub-matrix made up of the specified portion of
// this matrix.
//
// Input Arguments:
// StartRow = const unsigned int& specifying the starting row
// StartColumn = const unsigned int& specifying the starting column
// SubRows = const unsigned int& specifying the number of rows
// SubColumns = const unsigned int& specifying the number of columns
//
// Output Arguments:
// None
//
// Return Value:
// Matrix contining the specified sub-matrix
//
//==========================================================================
Matrix Matrix::GetSubMatrix(const unsigned int &startRow, const unsigned int &startColumn,
const unsigned int &subRows, const unsigned int &subColumns) const
{
assert(startRow + subRows <= rows && startColumn + subColumns <= columns);
Matrix subMatrix(subRows, subColumns);
unsigned int i, j;
for (i = 0; i < subRows; i++)
{
for (j = 0; j < subColumns; j++)
subMatrix.elements[i][j] = elements[i + startRow][j + startColumn];
}
return subMatrix;
}
Matriz Matriz::getSubMatrix(int i_0, int j_0, int i, int j)
{
if (i<i_0) swap(i, i_0);
if (j<j_0) swap(j, j_0);
Matriz subMatrix(i-i_0 + 1, j-j_0 + 1);
for (int ii=i_0;ii<=i;ii++)
{
for (int jj=j_0;jj<=j;jj++)
{
subMatrix.addValue(values[ii][jj]);
}
}
return subMatrix;
}
int maxRectSum(vector<vector<int> > &matrix)
{
int maxSum = 0;
if (matrix.empty())
return 0;
for (int i = 0; i < matrix.size(); i ++)
{
vector<int> subMatrix(matrix[0].size(), 0);
for (int j = i; j < matrix.size(); ++ j)
{
for (int k = 0; k < matrix[0].size(); ++ k)
{
subMatrix[k] += matrix[j][k];
}
}
maxSum = max(maxSum, maxConsSum(subMatrix));
}
return maxSum;
}
/*
Main-Funktion
*/
int main(int argc, char *argv[]) {
// Zufallszahlengenerator initialisieren
srand(time(NULL));
matrix a = initMatrixRand(5,5);
matrix b = initMatrixRand(5,5);
printf("a =\n"); prettyPrint(a);
printf("\nb =\n"); prettyPrint(b);
matrix c = addMatrix(a, b);
printf("\na + b =\n");
prettyPrint(c);
freeMatrix(c);
c = subMatrix(a, b);
printf("\na - b =\n");
prettyPrint(c);
freeMatrix(c);
c = multMatrix(a, b);
printf("\na * b =\n");
prettyPrint(c);
freeMatrix(c);
c = transposeMatrix(a);
printf("\na^T =\n");
prettyPrint(c);
freeMatrix(c);
printf("\ndet(a) = %.2f\n", determinante(a));
printf("detQuick(a) = %.2f\n\n", detQuick(a));
printf("\ndet(b) = %.2f\n", determinante(b));
printf("detQuick(b) = %.2f\n\n", detQuick(b));
freeMatrix(a);
freeMatrix(b);
return 0;
}
void Matrix::extractQ ( const Vector& tau, const Matrix& qr ) {
const size_t
rows = countRows(),
cols = countColumns(),
qrRows = qr.countRows(),
qrCols = qr.countColumns(),
tauDims = tau.countDimensions();
assert( qrRows >= qrCols );
assert( qrCols == tauDims );
assert( rows == qrRows );
assert( cols == qrCols || cols == qrRows );
fill( 0.0 );
Vector diagonal = diagonalVector();
diagonal.fill( 1.0 );
for ( size_t col = qrCols; 0 < col--; ) {
Matrix affected = subMatrix( col, col, rows - col, cols - col );
affected.householderTransform(
tau.get( col ),
const_cast<Matrix&>( qr ).columnVector( col ).subVector( col, rows - col )
);
}
}
/**
****************************************************************************************************
\fn void Matrix4Inverse( Matrix &i_matrix, Matrix &o_matrix )
\brief Calculate the inverse of matrix4x4 i_matrix
\param i_matrix input matrix
\param o_matrix output matrix
\return NONE
****************************************************************************************************
*/
void GameEngine::Math::Matrix::Matrix4Inverse( Matrix &i_matrix, Matrix &o_matrix )
{
float determinant = 0.0f;
Matrix subMatrix( 3, 3 );
INT8 sign = 0;
FUNCTION_START;
assert( i_matrix._u32Column == 4 );
assert( i_matrix._u32Row == 4 );
assert( o_matrix._u32Column == 4 );
assert( o_matrix._u32Row == 4 );
determinant = Matrix4Determinant( i_matrix );
if( Utilities::Math::AreRelativelyEqual(determinant, 0.0f, 20) )
{
o_matrix = i_matrix.Identity();
FUNCTION_FINISH;
return;
}
for( UINT8 i = 0; i < 4; ++i )
{
for( UINT8 j = 0; j < 4; ++j )
{
sign = 1 - ( (i + j) % 2 ) * 2;
Matrix4SubMatrix( i_matrix, subMatrix, i, j );
o_matrix( j, i ) = ( Matrix3Determinant(subMatrix) * sign ) / determinant;
}
}
FUNCTION_FINISH;
return;
}
/**
****************************************************************************************************
\fn float Matrix4Determinant( Matrix &i_matrix )
\brief Calculate the determinant of matrix4x4 i_matrix
\param i_matrix input matrix
\return float
\retval The determinant of input matrix
****************************************************************************************************
*/
float GameEngine::Math::Matrix::Matrix4Determinant( Matrix &i_matrix )
{
float retVal = 0.0f;
float determinant = 0.0f;
INT8 sign = 1;
UINT8 n = 0;
Matrix subMatrix( 3, 3 );
FUNCTION_START;
assert( i_matrix._u32Column == 4 );
assert( i_matrix._u32Row == 4 );
for( n = 0; n < 4; n++, sign *= -1 )
{
Matrix4SubMatrix( i_matrix, subMatrix, 0, n );
determinant = Matrix3Determinant( subMatrix );
retVal += (i_matrix(0, n) * determinant * sign);
}
FUNCTION_FINISH;
return retVal;
}
Matrix* BlockMatrix::block2distributed()
{
if(this->nBlockRows < 1 || this->nBlockCols < 1)
{
misc.error("Error: An internal error was happened. A cyclic matrix cannot be generated from an empty block matrix.", 0);
}
Matrix* result = new Matrix(MATRIX_DEFAULT_DISTRIBUTION, this->nGlobRows, this->nGlobCols);
result->fillWithConstant(0.);
int rowShift = 0;
for(int r = 0; r < this->nBlockRows; r++)
{
int colShift = 0;
for(int c = 0; c < this->nBlockCols; c++)
{
result->add( this->m[r][c], 1., 1., subMatrix(rowShift, colShift ,this->m[r][c]->nGlobRows, this->m[r][c]->nGlobCols), subMatrix(this->m[r][c]) );
colShift += this->m[r][c]->nGlobCols;
}
rowShift += this->m[r][0]->nGlobRows;
}
return result;
}
FgMatrixV colVec(uint n) const {return subMatrix(0,n,nrows,1); };
//This function is used to recognize continuous SLR in a off-line way.
//Since the data should be read in all at once, the class gcmCont is not necessary used.
int gcm::patchRun_continuous_PQ(vector<SLR_ST_Skeleton> vSkeletonData, vector<Mat> vDepthData, vector<IplImage*> vColorData,
int *rankIndex, double *rankScore)
{
int window = 40;
int kernelFeatureDim = NClass*NTrainSample;
//Computing features
cout<<"Computing features..."<<endl;
oriData2Feature(vSkeletonData, vDepthData, vColorData);
//////////////////////////////////////////////////////////////////////////
//Compute P and Q all at once.
vector<double*> P;
vector<double**> Q;
cout<<"Computing P and Q..."<<endl;
//computePQ(P, Q, vColorData.size());
//white
for (int d=0; d<nDimension; d++)
{
double dimAve = 0.0;
for (int f=0; f<nFrames; f++)
{
dimAve += feature_ori[f][d];
}
dimAve /= nFrames;
for (int f=0; f<nFrames; f++)
{
feature_ori[f][d] -= dimAve;
}
}
//Compute P and Q.
int nFrames_PQ = vColorData.size();
for (int i=0; i<nFrames_PQ; i++)
{
cout<<"Current "<<i<<"/"<<nFrames_PQ<<endl;
//Compute P
double* tempP = new double[featureDim];
for (int j=0; j<featureDim; j++)
{
tempP[j] = 0;
for (int k=0; k<i; k++)
{
tempP[j] += feature_ori[k][j];
}
}
P.push_back(tempP); //To release it when reaching the end of a sentence
//Compute Q
double** tempQ;
tempQ = newMatrix(featureDim, featureDim);
for (int f1=0; f1<featureDim; f1++)
{
for (int f2=0; f2<featureDim; f2++)
{
tempQ[f1][f2] = 0;
for (int l=0; l<i; l++)
{
tempQ[f1][f2] += feature_ori[f1][l]*feature_ori[f2][l];
}
}
}
Q.push_back(tempQ); //To release it when reaching the end of a sentence
}
//////////////////////////////////////////////////////////////////////////
double** C = newMatrix(featureDim, featureDim);
double* p_temp = new double[featureDim]; //The deltaP
double** Pm = newMatrix(featureDim, featureDim);
for (int i=window; i<vColorData.size()-window; i++)
{
int begin = i-window/2;
int end = i+window/2;
//The matrix from p
for (int pf=0; pf<featureDim; pf++)
{
p_temp[pf] = P[end][pf]-P[begin][pf];
}
vector2matrix(p_temp, p_temp, Pm,featureDim);
//Compute the covariance matrix
for (int l=0; l<featureDim; l++)
{
for (int m=0; m<featureDim; m++)
{
C[l][m] = ((Q[end][l][m]-Q[begin][l][m])-Pm[l][m]/(end-begin+1))/(end-begin);
}
}
//Regularization term added
for (int d=0; d<nDimension; d++)
{
for (int d2=0; d2<nDimension; d2++)
{
//C[d][d2] /= nFrames;
if (d == d2)
{
C[d][d2] += 0.001;
}
}
}
//The subspace matrix
PQsubspace(C, gcm_subspace);
//For debug
ofstream foutDebug;
foutDebug.open("..\\output\\debug.txt");
for (int i=0; i<featureDim; i++)
{
for (int j=0; j<subSpaceDim; j++)
{
foutDebug<<gcm_subspace[i][j]<<"\t";
}
foutDebug<<"\n";
}
foutDebug << flush;
foutDebug.close();
//The SVM classification
x[0].index = 0;
for (int j=0; j<kernelFeatureDim; j++)
{
subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim);
x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim);
x[j+1].index=j+1;
}
x[kernelFeatureDim+1].index=-1;
int testID = svm_predict_probability(myModel, x, prob_estimates);
cout<<"Frame: "<<i<<"/"<<vColorData.size()-window<<" Result: "<<testID<<endl;
}
delete[] p_temp;
deleteMatrix(Pm,featureDim);
deleteMatrix(C, featureDim);
//To delete P and Q
return 1;
// gcmSubspace();
//
// x[0].index = 0;
// for (int j=0; j<kernelFeatureDim; j++)
// {
// subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim);
// x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim);
// x[j+1].index=j+1;
// }
// x[kernelFeatureDim+1].index=-1;
//
// int testID = svm_predict_probability(myModel, x, prob_estimates);
//
// //Sort and get the former 5 ranks.
// vector<scoreAndIndex> rank;
// for (int i=0; i<myModel->nr_class; i++)
// {
// scoreAndIndex temp;
// temp.index = myModel->label[i];
// temp.score = prob_estimates[i];
// rank.push_back(temp);
// }
// sort(rank.begin(),rank.end(),comp);
//
// for (int i=0; i<5; i++)
// {
// rankIndex[i] = rank[i].index;
// rankScore[i] = rank[i].score;
// }
//
//
// //Release
// nFrames = 0;
//
// return testID;
}
void main(int argc, char *argv[]){
int id,np;
int dim1,dim2,dim3;
double *a,*b,*c;
double *adist,*bdist,*cdist;
MPI_Init(&argc,&argv);
sscanf(argv[1],"%d",&dim1);
sscanf(argv[2], "%d",&dim2);
sscanf(argv[3],"%d",&dim3);
MPI_Comm_rank(MPI_COMM_WORLD, &id);
MPI_Comm_size(MPI_COMM_WORLD, &np);
if(id==0){
a=malloc(sizeof(double)*dim1*dim2);
b=malloc(sizeof(double)*dim2*dim3);
matrizAleatoria(dim1,dim2,a);
matrizAleatoria(dim2,dim3,b);
b[1] = 8;
b[5] = 10;
b[15] = 0;
printf("A = \n");
impMat(dim1,dim2,a);
printf("B = \n");
impMat(dim1,dim2,b);
}
adist=malloc(sizeof(double)*(dim1*dim2)/np);
bdist=malloc(sizeof(double)*(dim2*dim3)/np);
MPI_Scatter(a,(dim1*dim2)/np,MPI_DOUBLE,adist,(dim1*dim2)/np,MPI_DOUBLE,0,MPI_COMM_WORLD);
MPI_Scatter(b,(dim2*dim3)/np,MPI_DOUBLE,bdist,(dim2*dim3)/np,MPI_DOUBLE,0,MPI_COMM_WORLD);
//impMat(dim1/np,dim2,adist);
//impMat(dim2/np,dim3,bdist);
char TRANSA, TRANSB;
int M, N, K;
double ALPHA, BETA;
TRANSA = 'N';
TRANSB = 'N';
ALPHA = 1.0;
BETA = 1.0;
M=dim1/np;
K=dim2/np;
N=dim3;
cdist=malloc(sizeof(double)*(dim1*dim3)/np);
zerosMat(dim1/np,dim3,cdist);
int j;
double *asub;
asub=malloc(sizeof(double)*(dim1/np*dim2/np));
for(j=0;j<np;j++){
asub=subMatrix(dim1,dim2,mod(id-j,np),np,adist);
dgemm_(&TRANSA, &TRANSB, &N, &M, &K, &ALPHA, bdist, &N, asub, &K, &BETA, cdist, &N);
printf("---------------------------------------\n");
printf("i = %d\tj = %d\tnump = %d\n", id, j, mod(id-j,np));
printf("Aij = \n");
impMat(dim1/np,dim2/np,asub);
//printf("Ai = \n");
//impMat(dim1/np,dim2,adist);
printf("Bi = \n");
impMat(dim2/np,dim3,bdist);
printf("Ci (resultado parcial) = \n");
impMatTransp(dim3,dim1/np,cdist);
printf("---------------------------------------\n");
MPI_Send(bdist, (dim2*dim3)/np, MPI_DOUBLE, mod(id + 1,np), mod(id-j,np),MPI_COMM_WORLD);
MPI_Recv(bdist,(dim2*dim3)/np, MPI_DOUBLE, mod(id - 1,np), mod(id-j-1,np), MPI_COMM_WORLD,MPI_STATUS_IGNORE);
}
if(id==0){
free(a);
free(b);
}
free(adist);
free(bdist);
free(asub);
MPI_Finalize();
}
void mainfunction(int *network,int *minNetwork,int size)
{
//calculate the b w
auto dn = genDN(network,size);
auto bw = calmaxBW(dn); //76,272
//std::cout<<"B = "<<bw.first<<" W = "<<bw.second<<std::endl;
//calculate the supplement node
int* supp_node = new int [size];
for(int i = 0; i < size; ++i)
supp_node[i] = 0;
int *supp_network = subMatrix(network,minNetwork,size,size);
for(int i = 0; i < size; ++i){
for(int j = 0; j < size; ++j){
if(supp_network[i*size+j] != 0){
supp_node[j] ++; //outdegree!!!
supp_node[i] ++; //indegree!!!
}
}
}
//calculate the b d
int *node_num = new int[size];
for(int i = 0; i < size; ++i){
node_num[i] = i+1; //set node index
}
double *bd = calBD(network,size);
//pop sort
for(int i = 0; i < size; ++i){
for(int j = 0; j < size; ++j){
if(bd[i]>bd[j]){
swap2(bd[i],bd[j]);
swap2(node_num[i],node_num[j]);
swap2(supp_node[i],supp_node[j]);
}
}
}
//print the result
//for(int i = 0; i < size; ++i){
//std::cout<<std::setw(2)<<node_num[i]<<" "<<std::setw(10)<<bd[i]<<" "<<supp_node[i]<<std::endl;
//}
//calculate the correlation
double correlation = 0;
double bd_total = 0;
double indegree_total = 0;
double deviation = 0;
for(int i = 1; i < size; ++i){
bd_total += bd[i] * bd[i];
indegree_total += supp_node[i] * supp_node[i];
}
for(int i = 1; i < size; ++i){
correlation += (abs2(bd[i])/sqrt(bd_total)) * (supp_node[i]/sqrt(indegree_total));
deviation += supp_node[i] * supp_node[i];
}
//std::cout<<sqrt(deviation)/indegree_total<<" "<<correlation<<std::endl;
std::cout<<bw.first<<" "<<correlation<<std::endl;
//else std::cout<<0.1<<" "<<correlation<<std::endl;
delete supp_node;
delete node_num;
delete bd;
}
void
gaussianEstimator_Est (Matrix *xEst, Matrix *CEst, Matrix *y, Matrix *Cv, Matrix (*hfun)(Matrix m), Matrix *m_opt)
{
//printMatrix(*xEst);
//printMatrix(*CEst);system("PAUSE");
Matrix tmp = sizeOfMatrix(*m_opt);
float D = elem(tmp,0,1)+1; //printf("%f\n", D);
freeMatrix(tmp);
float w_opt = 1/D; //printf("%f\n", w_opt);
tmp = sizeOfMatrix(*xEst);
float Nx = elem(tmp,0,0); // printf("%f\n", Nx);
freeMatrix(tmp);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; // printf("%f\n", w);system("PAUSE");
// Eigenvectors, Eigenvalues
tmp = sizeOfMatrix(*CEst);
int dimC = elem ( tmp, 0, 0 );
freeMatrix(tmp);
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val ); //printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val); //printMatrix(m1); system("PAUSE");
freeMatrix(Vec);
freeMatrix(Val);
//* rotate & scale samples: m = m1*S
Matrix m = scaledSamplePoints(m1, *m_opt); // printMatrix(m); system("PAUSE");
Matrix mxDiracs = mulScalarMatrix(1, m);
//* x = x*ones(1,d)
Matrix x = fillMatrix(*xEst, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ) ; //printMatrix(m);
freeMatrix(tmp);
//% Predicted Measurements
//* hfun
// yPredDiracs = feval(hfun, m, [], [], t);
// yPred = w*sum(yPredDiracs, 2);
Matrix yPredDiracs = (*hfun) (m); //printMatrix(yPredDiracs );
Matrix yPredDiracsSum = sumMatrix(yPredDiracs, 2);
Matrix yPred = mulScalarMatrix(w, yPredDiracsSum);
// myDiracs = yPredDiracs-repmat(yPred, 1, d);
tmp = fillMatrix(yPred, d);
Matrix myDiracs = subMatrix(yPredDiracs, tmp);
freeMatrix(tmp);
//* CPred = w_opt*mxDiracs*mxDiracs';
// Matrix CPred = mulScalarMatrix( w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)) );
// Matrix CPred = *CEst;
// Cxy = w_opt*mxDiracs*myDiracs';
Matrix tmp1 = transposeMatrix(myDiracs);
Matrix tmp2 = mulMatrix(mxDiracs, tmp1);
Matrix Cxy = mulScalarMatrix( w_opt, tmp2);
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = w_opt*myDiracs*myDiracs'+Cv;
tmp1 = transposeMatrix(myDiracs);
tmp2 = mulMatrix(myDiracs, tmp1);
Matrix tmp3 = mulScalarMatrix( w_opt, tmp2);
Matrix Cy = addMatrix( tmp3 , *Cv );
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
// K = Cxy / Cy;
tmp = invertCovMatrix(Cy);
Matrix K = mulMatrix( Cxy, tmp);
freeMatrix(tmp);
// I = y - yPred;
Matrix I = subMatrix( *y, yPred );
// xEst = xPred + K*I;
tmp = mulMatrix( K, I );
Matrix tmp23 = addMatrix( *xEst, tmp);
appMatrix(*xEst,0,5,0,0, tmp23,0,5,0,0);
freeMatrix(tmp);
// CEst = CPred - K*Cy*K';
tmp1 = mulMatrix(K, Cy);
tmp2 = transposeMatrix(K);
tmp3 = mulMatrix( tmp1, tmp2);
Matrix tmp24 = subMatrix(*CEst, tmp3);
appMatrix(*CEst,0,5,0,5, tmp24,0,5,0,5);
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp24);
freeMatrix(m1);
freeMatrix(m);
freeMatrix(mxDiracs);
freeMatrix(x);
freeMatrix(yPredDiracs);
freeMatrix(yPredDiracsSum);//
freeMatrix(yPred);//
freeMatrix(myDiracs);
freeMatrix(Cxy);
freeMatrix(Cy);
freeMatrix(K);//
freeMatrix(I);//
freeMatrix(tmp23);
}
void
gaussianEstimator_Pred_decomp ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, float *dt, Matrix *m_opt)
{
float r;
Matrix sizeMopt;
Matrix xn = zeroMatrix(3,1);
Matrix Cn = zeroMatrix(3,3);
Matrix xl = zeroMatrix(9,1);
Matrix Cl = zeroMatrix(9,9);
Matrix Cnl = zeroMatrix(3,9);
Matrix Cnl_T;
Matrix Cn_i;
Matrix CLN;
Matrix sizeCn;
Matrix Vec;
Matrix Val;
Matrix m1;
Matrix m;
Matrix x;
Matrix A;
Matrix Hi = zeroMatrix(12,9);
Matrix Cy = zeroMatrix(12, 12);
Matrix muy = zeroMatrix(12, 1);
Matrix zeros33 = zeroMatrix(3,3);
Matrix eye33 = unitMatrix(3,3);
Matrix Mat;
Matrix H;
Matrix gi = zeroMatrix(12,1);
Matrix Rot_vec = zeroMatrix(3,1);
Matrix mui;
Matrix muiy;
Matrix Ciy;
Matrix tmp;
Matrix tmp1;
Matrix tmp2;
Matrix tmp3;
Matrix tmp4;
Matrix tmp5;
Matrix tmp6;
Matrix tmp7;
Matrix tmp8;
Matrix tmpHi;
sizeMopt = sizeOfMatrix(*m_opt); //printf("%f\n",*dt);
float D = elem(sizeMopt,0,1)+1; //printf("%f\n", D);
freeMatrix(sizeMopt);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = 3;
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
//xn = xEst(4:6); % Rotation vector
appMatrix(xn, 0, 2, 0, 0, *xEst, 3, 5, 0, 0); //printMatrix(xn);system("PAUSE");
//Cn = CEst(4:6,4:6);
appMatrix(Cn, 0, 2, 0, 2, *CEst, 3, 5, 3, 5); //printMatrix(Cn);system("PAUSE");
//xl = [xEst(1:3) ; xEst(7:12)]; % Translation, angular velocity, linear velocity
appMatrix(xl, 0, 2, 0, 0, *xEst, 0, 2, 0, 0);
appMatrix(xl, 3, 8, 0, 0, *xEst, 6, 11, 0, 0); //printMatrix(xl);system("PAUSE");
//Cl = [CEst(1:3,1:3) CEst(1:3,7:12);
// CEst(7:12,1:3) CEst(7:12,7:12)] ;
appMatrix(Cl, 0, 2, 0, 2, *CEst, 0, 2, 0, 2);
appMatrix(Cl, 0, 2, 3, 8, *CEst, 0, 2, 6, 11);
appMatrix(Cl, 3, 8, 0, 2, *CEst, 6, 11, 0, 2);
appMatrix(Cl, 3, 8, 3, 8, *CEst, 6, 11, 6, 11); //printMatrix(Cl);system("PAUSE");
//Cnl = [CEst(4:6,1:3) CEst(4:6,7:12)];
appMatrix(Cnl, 0, 2, 0, 2, *CEst, 3, 5, 0, 2);
appMatrix(Cnl, 0, 2, 3, 8, *CEst, 3, 5, 6, 11); //printMatrix(Cnl);system("PAUSE");
//CLN = Cl - Cnl'*inv(Cn)*Cnl;
Cnl_T = transposeMatrix(Cnl);
// printMatrix(Cn);system("PAUSE");
Cn_i = invertCovMatrix(Cn); //printMatrix(Cn_i);system("PAUSE");
tmp = mulMatrix( Cnl_T, Cn_i);
tmp7 = mulMatrix(tmp, Cnl);
CLN = subMatrix ( Cl, tmp7); //printMatrix(CLN);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp7);
// Eigenvectors, Eigenvalues
sizeCn = sizeOfMatrix(Cn);
int dimC = elem ( sizeCn, 0, 0 );
freeMatrix(sizeCn);
Vec = zeroMatrix(dimC, dimC);
Val = zeroMatrix(dimC, dimC);
eig ( &Cn, &Vec, &Val ); //printMatrix(Cn);printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
m1 = mulMatrix(Vec, Val); //printMatrix(m1);system("PAUSE");
// rotate & scale samples: m = m1*S
m = scaledSamplePoints(m1, *m_opt); //printMatrix(m);system("PAUSE");
// x = x*ones(1,d)
x = fillMatrix(xn, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ); //printMatrix(m);system("PAUSE");
freeMatrix(tmp);
//A = [[eye(3,3),t*eye(3,3)];[zeros(3,3),eye(3,3)]];
A = unitMatrix(6,6);
setElem(A, 0, 3, *dt);
setElem(A, 1, 4, *dt);
setElem(A, 2, 5, *dt); //printMatrix(A);system("PAUSE");
for (i=0; i<d; i++)
{
//gi = [zeros(3,1); m(:,i); zeros(6,1)];
setElem(gi, 3, 0, elem(m, 0, i));
setElem(gi, 4, 0, elem(m, 1, i));
setElem(gi, 5, 0, elem(m, 2, i)); //printMatrix(gi);system("PAUSE");
//Rot_vec = m(:,i);
setElem(Rot_vec, 0, 0, elem(m, 0, i));
setElem(Rot_vec, 1, 0, elem(m, 1, i));
setElem(Rot_vec, 2, 0, elem(m, 2, i)); //printMatrix(Rot_vec);system("PAUSE");
//r = norm(Rot_vec);
r = sqrtf( powf((elem(Rot_vec,0,0)),2) + powf((elem(Rot_vec,1,0)),2) + powf((elem(Rot_vec,2,0)),2) ); //printf("%f\n",r);
H = zeroMatrix(3,3);
if (fmod(r, 2*pi) == 0)
{
Mat = unitMatrix(3,3);
}
else
{
// build skew symmetric Matrix
setElem(H, 0, 1, -elem(Rot_vec,2,0));
setElem(H, 0, 2, elem(Rot_vec,1,0));
setElem(H, 1, 0, elem(Rot_vec,2,0));
setElem(H, 1, 2, -elem(Rot_vec,0,0));
setElem(H, 2, 0, -elem(Rot_vec,1,0));
setElem(H, 2, 1, elem(Rot_vec,0,0)); //printMatrix(H);system("PAUSE");
// Bortz equation
// Mat = eye(3,3) + 0.5*H + (1- r*sin(r)/( 2*(1-cos(r))))/r^2*H*H;
// already declared Mat = unitMatrix(3,3);
tmp1 = mulScalarMatrix(0.5, H);
tmp4 = addMatrix( eye33 , tmp1 );
tmp2 = mulMatrix(H, H);
tmp3 = mulScalarMatrix( (1-(r*sin(r)/(2*(1-cos(r)))))/powf(r,2), tmp2);
Mat = addMatrix( tmp4, tmp3);
//printMatrix(Mat);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
}
//Hi = [[A(1:3,1:3) zeros(3,3) A(1:3,4:6)];
// [zeros(3,3), t*Mat, zeros(3,3)];
// [zeros(3,3), eye(3,3), zeros(3,3)];
// [A(4:6,1:3),zeros(3,3), A(4:6,4:6)]];
appMatrix( Hi, 0, 2, 0, 2, A, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 6, 8, A, 0, 2, 3, 5 );
appMatrix( Hi, 3, 5, 0, 2, zeros33, 0, 2, 0, 2 );
tmpHi = mulScalarMatrix(*dt, Mat);
appMatrix( Hi, 3, 5, 3, 5, tmpHi, 0, 2, 0, 2 );
freeMatrix(tmpHi);
appMatrix( Hi, 3, 5, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 0, 2, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 3, 5, eye33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 0, 2, A, 3, 5, 0, 2 );
appMatrix( Hi, 9, 11, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 6, 8, A, 3, 5, 3, 5 ); //printMatrix(Hi);system("PAUSE");
// mui = xl + Cnl'*inv(Cn)*(m(:,i)-xn); //m(:,i) -> Rot_vec
tmp = mulMatrix(Cnl_T, Cn_i );
tmp1 = subMatrix(Rot_vec, xn);
tmp2 = mulMatrix(tmp, tmp1);
mui = addMatrix(xl, tmp2);
freeMatrix(tmp);
freeMatrix(tmp1);
freeMatrix(tmp2); //printMatrix(mui);system("PAUSE");
// muiy = gi + Hi * mui;
tmp = mulMatrix(Hi, mui);
muiy = addMatrix( gi, tmp); //printMatrix(muiy);system("PAUSE");
freeMatrix(tmp);
// Ciy = Hi *CLN *Hi';
tmp1 = mulMatrix(Hi, CLN);
tmp2 = transposeMatrix(Hi);
Ciy = mulMatrix( tmp1, tmp2); //printMatrix(Ciy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = Cy + (w*Ciy + w_opt*muiy*muiy');
tmp3 = mulScalarMatrix(w, Ciy);
tmp1 = transposeMatrix(muiy);
tmp2 = mulMatrix(muiy, tmp1);
tmp4 = mulScalarMatrix( w_opt, tmp2 );
tmp5 = addMatrix( tmp3, tmp4 );
tmp6 = addMatrix( Cy, tmp5);
appMatrix(Cy,0,Cy->height-1,0,Cy->width-1,tmp6, 0,tmp6->height-1,0,tmp6->width-1); //printMatrix(Cy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
freeMatrix(tmp5);
freeMatrix(tmp6);
// muy = muy + w*muiy;
tmp = mulScalarMatrix( w, muiy );
tmp2 = addMatrix( muy, tmp );
appMatrix(muy,0,muy->height-1,0,muy->width-1, tmp2, 0, tmp2->height-1, 0, tmp2->width-1); //printMatrix(muy);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp2);
freeMatrix(H);
freeMatrix(Mat);
freeMatrix(mui);//
freeMatrix(muiy);//
freeMatrix(Ciy);
}
appMatrix(*xEst, 0, 11, 0, 0, muy, 0, 11, 0, 0 ); //printMatrix(muy);system("PAUSE");
//CEst = Cy - muy*muy' * w_opt/w + Cw;
tmp1 = transposeMatrix(muy);
tmp2 = mulMatrix(muy, tmp1);
tmp5 = mulScalarMatrix( w_opt/w, tmp2 );
tmp6 = subMatrix(Cy, tmp5);
tmp8 = addMatrix( tmp6, *Cw); //printMatrix(*CEst);system("PAUSE");
appMatrix(*CEst,0,11,0,11, tmp8, 0,11,0,11 ); //printMatrix(tmp8);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp5);
freeMatrix(tmp6);
freeMatrix(tmp8);
freeMatrix(muy);//
freeMatrix(zeros33);//
freeMatrix(Vec);
freeMatrix(Val);
freeMatrix(Cy);
freeMatrix(xn);
freeMatrix(Cn);
freeMatrix(xl);
freeMatrix(Cl);//
freeMatrix(Cnl);
freeMatrix(Cnl_T);
freeMatrix(Cn_i);
freeMatrix(CLN);//
freeMatrix(m1);
freeMatrix(m);//
freeMatrix(x);
freeMatrix(A);
freeMatrix(eye33);
freeMatrix(Hi);
freeMatrix(gi);
freeMatrix(Rot_vec);
} /* End gaussianPred_decomp */
int main (int argc, const char ** argv)
{
ProgramArgument program_args=ProcessArguments(argc,argv);
bool print_ali=program_args.print_ali;
ParseMatrix submatParser(program_args.matrixname);
#ifdef DEBUG
std::cout << "Print Matrix\n";
submatParser.print();
#endif
SubstitutionMatrix subMatrix(submatParser.scores, submatParser.row_index);
#ifdef DEBUG
std::cout << "Print SubMatrix!\n";
subMatrix.print();
#endif
SequenceLibrary sequences(program_args.seqlib);
const size_t max_seq_size = sequences.get_max_seq_size();
PairsLibrary pairs(program_args.pairs,&sequences);
GotohEight gotohAlgo(max_seq_size,&subMatrix, program_args.go, program_args.ge, program_args.mode);
SequenceEight sequenceQuery(max_seq_size, subMatrix.aa2short,subMatrix.short2aa);
SequenceEight sequenceTemplate(max_seq_size, subMatrix.aa2short,subMatrix.short2aa);
char buffer[2097152];
std::cout.rdbuf()->pubsetbuf(buffer, 2097152);
const char ** template_sequences=(const char **)memalign(16,(VEC_SIZE)*sizeof(const char *));
const char ** template_keys=(const char **)memalign(16,(VEC_SIZE)*sizeof(const char *));
std::map< std::string,std::vector<std::pair<std::string,size_t> > >::iterator it = pairs.pairs.begin();
for( ; it != pairs.pairs.end(); ++it ){
std::pair<std::string, size_t> query = sequences.get_sequence(it->first);
std::string query_key = it->first.c_str();
sequenceQuery.MapOneSEQToSEQ8(query.first.c_str());
std::vector<std::pair<std::string, size_t> > t_seq = it->second;
sort (t_seq.begin(), t_seq.end(), sort_seq_vector);
int elem_count=0;
size_t t_seq_size = t_seq.size();
for(std::vector<int>::size_type i = 0; i < t_seq_size ; i++) {
std::string template_key=t_seq[i].first;
template_keys[i%VEC_SIZE] = template_key.c_str();
template_sequences[i%VEC_SIZE] = sequences.get_sequence(template_key).first.c_str();
elem_count++;
if((i+1)%VEC_SIZE==0){
elem_count=0;
sequenceTemplate.MapSEQArrayToSEQ8(template_sequences, VEC_SIZE);
GotohEight::GotohMatrix matrix= gotohAlgo.calc_matrix(&sequenceQuery, &sequenceTemplate);
if(print_ali == true){
std::vector<char *> alignments=gotohAlgo.backtrace(matrix,&sequenceQuery,&sequenceTemplate);
for(size_t i = 0; i < 8; i++) {
if(program_args.check==true){
float check_score=gotohAlgo.calc_check_score(alignments[(i*2)],alignments[(i*2)+1]);
printf("check score: %.3f\n",check_score);
}
print_ali_result(alignments[(i*2)],alignments[(i*2)+1],matrix.score[i],query_key,template_keys[i]);
}
}else{
print_score(matrix,query_key,template_keys,VEC_SIZE);
}
}
}
if(elem_count!=0){
sequenceTemplate.MapSEQArrayToSEQ8(template_sequences, elem_count);
GotohEight::GotohMatrix matrix= gotohAlgo.calc_matrix(&sequenceQuery, &sequenceTemplate);
if(print_ali == true){
std::vector<char *> alignments=gotohAlgo.backtrace(matrix,&sequenceQuery,&sequenceTemplate);
for(size_t i = 0; i < elem_count; i++) {
if(program_args.check==true){
float check_score=gotohAlgo.calc_check_score(alignments[(i*2)],alignments[(i*2)+1]);
printf("check score: %.3f\n",check_score);
}
print_ali_result(alignments[(i*2)],alignments[(i*2)+1],matrix.score[i],query_key,template_keys[i]);
}
}else{
print_score(matrix,query_key,template_keys,elem_count);
}
}
}
std::cout.flush();
free(template_keys);
free(template_sequences);
return 0;
}
FgMatrixV rowVector(uint n) const {return subMatrix(n,0,1,ncols); };
int main()
{
char cmd[1000]={},split[5][100]={};
char *token;
int cmdSpt=0,i,returnA,returnB,returnTar,returnFunc,row,col,size;
double mul;
FILE *fileA=NULL,*fileB=NULL,*fileTar=NULL;
matrix matA,matB,matTar;
//prints welcome messages
system("cls");
printf("\n\n===Matrix manipulation program===\n\n");
printf("Type <help> for command list and usage\n\n");
do
{
printf("MATRIX> "); //command prompt
rewind(stdin); //rewind everything
scanf("%[^\n]",cmd); //get command (with spaces)
cmdSpt=0; //set sub command count to 0
token=strtok(cmd," "); //partition command to subcommand with spaces
while(token!=NULL&&cmdSpt<=5)
{
strcpy(split[cmdSpt],token); //save subcommands to split[]
cmdSpt++; //increase sub command count
token=strtok(NULL," ");
}
for(i=0;i<strlen(split[0]);i++) //set command to lowercase
split[0][i]=tolower(split[0][i]);
if(strcmp(split[0],"show")==0&&cmdSpt==2) //show command
{
returnA=openFile(&fileA,split[1],0); //call openFile() to open file to show
if(returnA==1) //failed to open
printf("Error: file %s failed to open.\n\n",split[1]);
else
{
matA=readMatrix(fileA); //read matrix and save to matA
showMatrix(matA,split[1]); //show matrix matA
}
}
else if(strcmp(split[0],"create")==0&&cmdSpt==4) //create command
{
returnA=openFile(&fileA,split[3],1); //call openFile() to create a new file
{
if(returnA==1) //failed to open
printf("Error: file %s failed to open.\n\n",split[3]);
else if(sscanf(split[1],"%d",&row)!=1||sscanf(split[2],"%d",&col)!=1) //incorrect matrix size entered
printf("Error: invalid matrix size.\n\n");
else if(row>50||col>50) //size too large
printf("Error: maximum matrix size is 50x50.\n\n");
else
{
createMatrix(row,col,fileA); //create a new matrix with specified size
printf("%dx%d matrix was successfully saved to %s.\n\n",row,col,split[3]);
}
}
}
else if(strcmp(split[0],"copy")==0&&cmdSpt==3) //copy command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileTar,split[2],1);
if(returnA==1) //failed to open source
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnA==0&&returnB==0) //passed
{
matA=readMatrix(fileA); //read source file
writeMatrix(matA,fileTar); //write to destination file
printf("Matrix from %s is successfully copied to %s.\n\n",split[1],split[2]);
}
}
else if(strcmp(split[0],"add")==0&&cmdSpt==4) //add command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=addMatrix(matA,matB,fileTar); //add matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Adder matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"subtract")==0||strcmp(split[0],"sub")==0)&&cmdSpt==4) //subtract command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=subMatrix(matA,matB,fileTar); //subtract matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Subtractor matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"multiply")==0||strcmp(split[0],"mul")==0)&&cmdSpt==4) //multiply command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=mulMatrix(matA,matB,fileTar); //multiply matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Multiplier matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"mulscalar")==0||strcmp(split[0],"muls")==0)&&cmdSpt==4) //multiply scalar command
{
returnA=openFile(&fileA,split[2],0); //open source and destination files
returnTar=openFile(&fileTar,split[3],1);
returnFunc=sscanf(split[1],"%lf",&mul); //convert multiplier to double
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnFunc!=1) //cannot convert multiplier to double (invalid)
printf("Error: invalid multiplier.\n\n");
if(returnA==0&&returnB==0&&returnFunc==1) //passed
{
matA=readMatrix(fileA); //read source file
mulScaMatrix(matA,mul,fileTar); //multiply with scalar and save to destination
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[2]);
printf("Multiply with %g\n\n",mul);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
}
else if((strcmp(split[0],"transpose")==0||strcmp(split[0],"trans")==0)&&cmdSpt==3) //transpose command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnTar=openFile(&fileTar,split[2],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnA==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read source file
transMatrix(matA,fileTar); //transpose matrix and save to destination
printf("New matrix is successfully saved to %s.\n\n",split[2]);
}
}
else if((strcmp(split[0],"identity")==0||strcmp(split[0],"iden")==0)&&cmdSpt==3) //identity matrix command
{
returnTar=openFile(&fileTar,split[2],1); //open destination file
returnFunc=sscanf(split[1],"%d",&size); //convert size to integer
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
else if(returnFunc!=1) //failed to convert size to integer (invalid)
printf("Error: invalid matrix size.\n\n");
else if(size>50) //size too large
printf("Error: maximum matrix size is 50x50.\n\n");
else
{
idenMatrix(size,fileTar); //create identity matrix with specified size and save to destination
printf("Identity matrix of size %d is successfully saved to %s.\n\n",size,split[2]);
}
}
else if(strcmp(split[0],"help")==0&&cmdSpt==1) //help command
{
displayHelp(); //display help
}
else if((strcmp(split[0],"clear")==0||strcmp(split[0],"cls")==0)&&cmdSpt==1) //clear screen command
{
system("cls"); //clear screen
}
else if((strcmp(split[0],"exit")==0||strcmp(split[0],"end")==0)&&cmdSpt==1) //exit command
{
printf("Exit\n"); //display exit message
}
else //invalid command
{
printf("Invalid command or incorrect command syntax, type <help> for command list and usage.\n\n");
}
if(fileA!=NULL) //close all file pointers in case they weren't closed by functions
fclose(fileA);
if(fileB!=NULL)
fclose(fileB);
if(fileTar!=NULL)
fclose(fileTar);
} while(strcmp(cmd,"exit")!=0&&strcmp(cmd,"end")!=0); //loop until exit
return 0;
}
double ObjectiveFunction::getDeterminant(Matrix<double> matrix)
{
double determinant = 0.0;
int numberOfRows = matrix.getNumberOfRows();
int numberOfColumns = matrix.getNumberOfColumns();
if(numberOfRows != numberOfColumns)
{
std::cout << "Error: NewtonMethod class. "
<< "getDeterminant(Matrix<double>) method." << std::endl
<< "Matrix must be square" << std::endl
<< std::endl;
exit(1);
}
if(numberOfRows == 0)
{
std::cout << "Error: NewtonMethod class. "
<< "getDeterminant(Matrix<double>) method." << std::endl
<< "Size of matrix is zero." << std::endl
<< std::endl;
exit(1);
}
else if(numberOfRows == 1)
{
// std::cout << "Warning: NewtonMethod class. "
// << "getDeterminant(Matrix<double>) method." << std::endl
// << "Size of matrix is one." << std::endl;
determinant = matrix[0][0];
}
else if(numberOfRows == 2)
{
determinant = matrix[0][0]*matrix[1][1] - matrix[1][0]*matrix[0][1];
}
else
{
for(int j1 = 0; j1 < numberOfRows; j1++)
{
Matrix<double> subMatrix(numberOfRows-1, numberOfColumns-1, 0.0);
for(int i = 1; i < numberOfRows; i++)
{
int j2 = 0;
for (int j = 0; j < numberOfColumns; j++)
{
if (j == j1)
{
continue;
}
subMatrix[i-1][j2] = matrix[i][j];
j2++;
}
}
determinant += pow(-1.0, j1+2.0)*matrix[0][j1]*getDeterminant(subMatrix);
}
}
return(determinant);
}
float Matrix4::det()
{
return mat[0][0]*subMatrix(0,0).det() - mat[1][0]*subMatrix(1,0).det() + mat[2][0]*subMatrix(2,0).det() - mat[3][0]*subMatrix(3,0).det();
}
int kalman_function(sensor_struct_t* sen,float_j_t dt){
/*
*
* system model
* omega = [ w_x w_y w_z ]^T
* x = [pi theta psi]^T
* used x_dot, C_inv, omega, x_k_1, x_k, dt,
*
* */
#define Def_x 0
#define Def_y 1
#define Def_z 2
#define Def_pi 0
#define Def_theta 1
#define Def_psi 2
//float_j_t dt;
//float_j_t B[3][3]={0,};
float_j_t res[3][3]={0,};
float_j_t res2[3][3]={0,};
float_j_t omega[3][3]={0,};
omega[0][0] = sen->gyroX*DEG_TO_RAD;
omega[1][0] = sen->gyroY*DEG_TO_RAD;
omega[2][0] = sen->gyroZ*DEG_TO_RAD;
static float_j_t x_k_1[3][3]={0,};
float_j_t x_k_p[3][3]={0,};
float_j_t pi,theta,psi;
pi = x_k_1[Def_pi][0];
theta = x_k_1[Def_theta][0];
psi = x_k_1[Def_psi][0];
if(cosf(theta)==0.0){
printf("cosf error\n ");
theta += 0.00001;
}
float_j_t C_inv[3][3]={0,};
C_inv[0][0] = 1*dt;
C_inv[0][1] = sinf(pi)*tanf(theta)*dt;
C_inv[0][2] = cosf(pi)*tanf(theta)*dt;
C_inv[1][0] = 0;
C_inv[1][1] = cosf(pi)*dt;
C_inv[1][2] = -sinf(pi)*dt;
C_inv[2][0] = 0;
C_inv[2][1] = (sinf(pi)/cosf(theta))*dt;
C_inv[2][2] = (cosf(pi)/cosf(theta))*dt;
//x_k = x_k_1 + C_inv * omega * dt
multiMatrix(C_inv,omega,res,3,3);
sumMatrix(res,x_k_1,x_k_p,3,3);
/* A_k*/
float_j_t A[3][3]={0,};
A[0][0] = 1 + (sinf(theta)*(omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt;
A[0][1] = ((omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt;
A[0][2] = 0;
A[1][0] = (-omega[1][0]*sinf(pi)-omega[2][0]*cosf(pi))*dt;
A[1][1] = 1;
A[1][2] = 0;
A[2][0] = ((omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt;
A[2][1] = (sinf(theta)*(omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt;
A[2][2] = 1;
float_j_t P_k_p[3][3]={0,};
static float_j_t P_k_1[3][3]={1,0,0,0,1,0,0,0,1};
float_j_t Q[3][3]={1,0,0,0,1,0,0,0,1};
//P_k = A* P_k_1 * A^T + B * Q * B^T
similarMatrix(A,P_k_1,res,3,3);
similarMatrix(C_inv,Q,res2,3,3);
sumMatrix(res,res2,P_k_p,3,3);
//Correct term
float_j_t H[3][3]={1,0,0,0,1,0,0,0,0};
float_j_t norm_a=9.8;
norm_a=sqrtf(powf(sen->accX,2)+powf(sen->accY,2)+powf(sen->accZ,2));
// printf("norm_a = %f\n",norm_a);
float_j_t R[3][3]={0,};
if((norm_a<10.8 )&&(norm_a>8.8)){
R[0][0]=1;
R[1][1]=1;
}else{
R[0][0]=1000;
R[1][1]=1000;
}
float_j_t res3[3][3] = {0,};
similarMatrix(H,P_k_p,res,3,3);
//printf("similarMatrix res\n");
//printMatrix(res,3,3);
sumMatrix(res,R,res2,3,3);
_2by2_inverseMatrix(res2,res,3,3);
//inverseMatrix(res2,res,3,3);
//result inverse matrix is res
//so kalman gain K = Pk* H^T* res
float_j_t K[3][3]={0,};
transMatrix(H,res3,3,3);
multiMatrix(P_k_p,res3,res2,3,3);
res2[0][2]=0;
res2[1][2]=0;
res2[2][0]=0;
res2[2][1]=0;
res2[2][2]=0;
// printf("res2 gain\n");
// printMatrix(res2,3,3);
// printf("res gain\n");
// printMatrix(res,3,3);
multiMatrix(res2,res,K,3,3);
// printf("kalman gain\n");
// printMatrix(K,3,3);
/* state variable, covaliance mat update*/
float_j_t x_k[3][3]={0,};
//calculate z_k roll, ptich, with gravitational euler angle calc.
float_j_t z_k[3][3] ={0,};
#if 1
z_k[0][0] = atan2f(sen->accY,sen->accZ);
z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) ));
#else
if(sen->accZ>=0){
z_k[0][0] = atan2f(sen->accY,sen->accZ);
z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) ));
}else if(sen->accZ<0){
omega[1][0] = -omega[1][0];
z_k[1][0] = -atan2f(sen->accX,sen->accZ);
z_k[0][0] = atan2f(-sen->accY, sqrtf( powf(sen->accX,2)+powf(sen->accZ,2) ));
}
#endif
//GravityToEuler(sen->accX, sen->accY, sen->accZ, z_k);
//printf("r %f p %f ",z_k[0][0]*180/3.14,z_k[1][0]*180/3.14);
multiMatrix(H,x_k_p,res,3,3);
subMatrix(z_k,res,res2,3,3);
multiMatrix(K,res2,res,3,3);
sumMatrix(x_k_p,res,x_k,3,3);
float_j_t P_k[3][3]={0,};
multiMatrix(K,H,res,3,3);
multiMatrix(res,P_k_p,res2,3,3);
subMatrix(P_k_p,res2,P_k,3,3);
/*insert prev value*/
insertMatrix(x_k, x_k_1,3,3);
insertMatrix(P_k, P_k_1,3,3);
/*Debug print*/
//printf("roll = %f , pitch = %f \n",x_k[0][0]*180.0/3.14,x_k[1][0]*180.0/3.14);
sen->roll=x_k[0][0]*RAD_TO_DEG;
sen->pitch=x_k[1][0]*RAD_TO_DEG;
return 0;
}
