//============================================================================//
//== 卡尔曼滤波 ==//
//============================================================================//
//==入口参数: 无 ==//
//==出口参数: 无 ==//
//==返回值: 无 ==//
//============================================================================//
void KalMan(void)
{
unsigned char i;
unsigned short j;
srand(SEED);
for (i=0; i<X_LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp2[i]; //零值初始化
}
for (i=0; i<P_LENGTH; i++)
{
tCov.PPreOpt[i] = Temp4[i]; //零值初始化
}
for (j=0; j<N; j++)
{
Watch1[j] = sin(2*3.14159265/100.0*j);
Y[0] = Watch1[j] + Random1(0, 0.4);
Watch2[j] = Y[0];
MatrixMul(A, tOpt.XPreOpt, X, A_ROW, X_ROW, X_COLUMN); // 基于系统的上一状态而预测现在状态; X(k|k-1) = A(k,k-1)*X(k-1|k-1)
MatrixCal1(A, tCov.PPreOpt, Temp4, SYS_ORDER);
MatrixAdd(Temp4, Q, P, P_ROW, P_COLUMN); // 预测数据的协方差矩阵; P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q
MatrixCal2(C, P, Temp1, C_ROW, C_COLUMN);
MatrixAdd(Temp1, R, Temp1, R_ROW, R_COLUMN);
Gauss_Jordan(Temp1, C_ROW);
MatrixTrans(C, Temp2, C_ROW, C_COLUMN);
MatrixMul(P, Temp2, Temp22, P_ROW, C_COLUMN, C_ROW);
MatrixMul(Temp22, Temp1, K, P_ROW, C_ROW, C_ROW); // 计算卡尔曼增益; K(k) = P(k|k-1)*C' / (C(k)*P(k|k-1)*C(k)' + R)
MatrixMul(C, X, Temp1, C_ROW, X_ROW, X_COLUMN);
MatrixMinus(Y, Temp1, Temp1, Y_ROW, Y_COLUMN);
MatrixMul(K, Temp1, Temp2, K_ROW, Y_ROW, Y_COLUMN);
MatrixAdd(X, Temp2, tOpt.XNowOpt, X_ROW, X_COLUMN); // 根据估测值和测量值计算当前最优值; X(k|k) = X(k|k-1)+Kg(k)*(Y(k)-C*X(k|k-1))
MatrixMul(K, C, Temp4, K_ROW, C_ROW, C_COLUMN);
MatrixMinus(I, Temp4, Temp4, I_ROW, I_COLUMN);
MatrixMul(Temp4, P, tCov.PNowOpt, I_ROW, P_ROW, P_COLUMN); // 计算更新后估计协防差矩阵; P(k|k) =（I-Kg(k)*C）*P(k|k-1)
for (i=0; i<X_LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
}
for (i=0; i<P_LENGTH; i++)
{
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
Watch3[j] = tOpt.XNowOpt[0];
}//end of for
}
// Kalman Update Step
// We "update" given what we get for data
// The filter will use the data given to lower our
// uncertainty (aka. covariance)
void KalmanUpdate(struct KalmanFilter *kal, struct Matrix meas)
{
struct Matrix y, S, extTran, K, Sinv, x_next, P_next;
CreateBlankMatrix(y);
CreateBlankMatrix(S);
CreateBlankMatrix(extTran);
CreateBlankMatrix(K);
CreateBlankMatrix(Sinv);
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CopyMatrix(&kal->measurementVector, meas);
// Find the difference between the move (measurement)
// and what we predicted (extraction * data)
MatrixMult(&y, kal->extractionMatrix, kal->meanVector);
MatrixSub(&y, kal->measurementVector, y);
// The Covariance of the move
MatrixMult(&S, kal->extractionMatrix, kal->covarianceMatrixX);
MatrixTranspose(&extTran, kal->extractionMatrix);
MatrixMult(&S, S, extTran);
MatrixAdd(&S, S, kal->covarianceMatrixZ);
// Kalman Gain
MatrixInv(&Sinv, S);
MatrixMult(&K, kal->covarianceMatrixX, extTran);
MatrixMult(&K, K, Sinv);
// Figure out mean and covariance results
MatrixMult(&x_next, K, y);
MatrixAdd(&x_next, kal->meanVector, x_next);
MatrixMult(&P_next, kal->covarianceMatrixX, extTran);
MatrixMult(&P_next, P_next, Sinv);
MatrixMult(&P_next, P_next, kal->extractionMatrix);
MatrixMult(&P_next, P_next, kal->covarianceMatrixX);
MatrixSub(&P_next, kal->covarianceMatrixX, P_next);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
// Delete matricies so we don't have memory leaks..
DeleteMatrix(&y);
DeleteMatrix(&S);
DeleteMatrix(&extTran);
DeleteMatrix(&K);
DeleteMatrix(&Sinv);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
}
// Kalman Filter Prediction Step
// We "predict" what the next data will be
// What this means is the filter's mean will change
// but we loose "certainty" about where the data is
// (aka. the covariance will increase in this step)
void KalmanPredict(struct KalmanFilter *kal)
{
struct Matrix x_next, P_next, F_trans;
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CreateBlankMatrix(F_trans);
// Calculate x_next (update guassian mean)
MatrixMult(&x_next, kal->updateMatrix, kal->meanVector);
MatrixAdd(&x_next, x_next, kal->moveVector);
// Calculate p_next (update guassian covariance);
MatrixMult(&P_next, kal->updateMatrix, kal->covarianceMatrixX);
MatrixTranspose(&F_trans, kal->updateMatrix);
MatrixMult(&P_next, P_next, F_trans);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
DeleteMatrix(&F_trans);
}
int main(int argc, char ** argv)
{
char op;
char mStr[1024];
matrix ans, m1, m2;
int sc = 0;
if(argc > 1){
op = getOp(argv[1]);
} else {
printf("Which operation: ");
op = getOp(NULL);
}
printf("First matrix:\n");
scanf("%s", mStr);
m1 = MatrixInit(mStr);
if(op == 'a' || op == 's' || op == 'm'){
printf("Second matrix:\n");
scanf("%s", mStr);
m2 = MatrixInit(mStr);
} else if(op == 'c') {
printf("Scalar multiple:\n");
scanf("%d", &sc);
}
switch(op){
case 'a':
ans = MatrixAdd(m1, m2);
break;
case 's':
ans = MatrixSub(m1, m2);
break;
case 'm':
ans = MatrixMul(m1, m2);
break;
case 'i':
ans = MatrixInv(m1);
break;
case 'c':
ans = MatrixSMul(m1, i2f(sc));
break;
default:
printf("Something went very wrong.\n");
return 1;
}
printf("Answer:\n");
MatrixPrint(ans);
MatrixFree(m1);
MatrixFree(ans);
if(op == 'a' || op == 's' || op == 'm'){
MatrixFree(m2);
}
return 0;
}
//============================================================================//
//== 卡尔曼滤波 ==//
//============================================================================//
//==入口参数: 无 ==//
//==出口参数: 无 ==//
//==返回值: 无 ==//
//============================================================================//
void KalMan(u16* in,u16* out)
{
unsigned char i;
// unsigned short k;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
// for (k=0; k<N; k++)
// {
Z[0] = in[0];//(float)ADCSampling();
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系统的上一状态而预测现在状态; X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 预测数据的协方差矩阵; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 计算卡尔曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根据估测值和测量值计算当前最优值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 计算更新后估计协防差矩阵; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
out[0]=(u16)(tOpt.XNowOpt[0]);
// }
}
task main()
{
// Initiate BNS Library
BNS();
// Create a 3x3 matrix of zeros
Matrix mat1;
CreateZerosMatrix(&mat1, 3, 3);
// Create a 3x3 matrix with some data in it
// Set location 1 down, 0 across, to be 16
Matrix mat2;
CreateMatrix(&mat2, "1.10 3.40 0; 5 3 2; 0 1 1.234");
SetMatrixAt(&mat2, 1, 0, 16);
// Creates a 3x3 identity matrix, then multiply it by 11
Matrix mat3;
CreateIdentityMatrix(&mat3, 3);
MatrixMultiplyScalar(&mat3, 11);
// Print matricies to the debugger console
PrintMatrix(&mat1);
PrintMatrix(&mat2);
PrintMatrix(&mat3);
// Matrix Examples:
// Matrix determinant
float det = MatrixDeterminant(&mat2);
writeDebugStreamLine("Matrix Det = %f", det);
// Matrix Inverse
Matrix inv;
MatrixInv(&inv, mat2);
writeDebugStream("Inverse ");
PrintMatrix(&inv);
// Matrix Multiplication
Matrix mult;
MatrixMult(&mult, mat2, mat3);
writeDebugStream("Multiply ");
PrintMatrix(mult);
// Matrix Addition
Matrix add;
MatrixAdd(&add, mat2, mat3);
writeDebugStream("Add ");
PrintMatrix(&add);
// Matrix Subtraction
Matrix sub;
MatrixSub(&sub, mat3, mat2);
writeDebugStream("Subtract ");
PrintMatrix(&sub);
}
//------------main function starts here-----------------------------------
void main()
{
int a[ROW][COL], b[ROW][COL],c[ROW][COL];
Insert(a,1);
Display(a);
Insert(b,2);
Display(b);
MatrixAdd(a,b,c);
cout<<"The sum of above matrix is"<<endl;
Display(c);
}
// the error
// d/dx softmax(x_i) = [label_i == 1] - p(x_i)
// cost function: NLL
// L = - 1/m forall i [label_i == 1] - p(x_i)
double SoftMaxLayer::backward(Data* expected_output) {
//TODO: check that m_labels is in one-hot encoding
m_backprop_error->copy_from(*m_output);
std::unique_ptr<Data> backprop_error_matrix = m_backprop_error->flatten_to_matrix();
std::unique_ptr<Data> expected_output_matrix = expected_output->flatten_to_matrix();
MatrixAdd(-1).execute(backprop_error_matrix.get(), expected_output_matrix.get());
//TODO: show we return the loss for each batch separately or just reduce it to the average?.
m_total_loss->copy_from(*m_output);
MatrixLog().execute(m_total_loss);
//mask to get only the probabilities of the true labels:
MatrixElementwiseMultiplication(m_total_loss, expected_output_matrix.get(), m_total_loss).execute();
long num_batches = m_output->get_num_samples();
double nll = -1./((float)num_batches)*DataSum().execute(m_total_loss);
std::cout << "NLL: " << nll << std::endl;
return nll;
}
//-------------------main function starts here------------------------------------
void main()
{
float a[ROW][COL], b[ROW][COL];
int m[2], n[2], choice,i,j,temp;
bool flag, flag1=true;
//-------------from the following the size of matrix is tested with proper input----------------------
cout<<"Enter the dimention of the 1st matrix :"<<endl;
for(i=0;i<2;i++)
{
flag = false;
while(!flag)
{
cin >> temp;
if(temp>0 && !cin.fail() && (cin.peek() == EOF || cin.peek() == '\n'))
{
flag = true;
m[i]=temp;
}
else
{
cout << "Please Enter valid data" << endl;
cin.clear();
cin.ignore(numeric_limits<streamsize>::max(), '\n');
}
}
}
cout<<"Enter the dimention of the 2nd matrix :"<<endl;
for(i=0;i<2;i++)
{
flag = false;
while(!flag)
{
cin >> temp;
if(temp>0 && !cin.fail() && (cin.peek() == EOF || cin.peek() == '\n'))
{
flag = true;
n[i]=temp;
}
else
{
cout << "Please Enter valid data" << endl;
cin.clear();
cin.ignore(numeric_limits<streamsize>::max(), '\n');
}
}
}
//---------------input for 1st matrix starts here----------------------
cout<<"Enter the 1st matrix"<<endl;
for(i=0;i<m[0];i++)
{
for(j=0;j<m[1];j++)
cin>>a[i][j];
}
//--------------input for 2nd matrix strts here-------------------------
cout<<"Enter the 2nd matrix"<<endl;
for(i=0;i<n[0];i++)
{
for(j=0;j<n[1];j++)
cin>>b[i][j];
}
//-------------Choice for Matrix operation to select------------------
while(1)
{
if(!flag1)
{
exit(0);
}
cout<<"Select the operation to perform with the above matrix \n 1. Addition\n 2. Subtraction\n 3. Multiplication"<<endl;
cin>>choice;
switch(choice)
{
case 1:
if(m[0]!=n[0] || m[1]!=n[1])
cout<<"Matrix cannot be added"<<endl;
else
MatrixAdd(a,b,m);
break;
case 2:
if(m[0]!=n[0] || m[1]!=n[1])
cout<<"Matrix cannot be subtracted"<<endl;
else
MatrixSub(a,b,m);
break;
case 3:
if(m[1]!=n[0])
cout<<"Matrix cannot be multiplied";
else
MatrixMul(a,b,m,n);
break;
default:
cout<<"Invalid Input!!"<<endl;
}
//-----------------Asked for continuity or to exit-------------------------------
cout<<"Enter 1 to try again or else 0 to exit : ";
cin>>flag1;
}
}
void Model::ComputeLinearDeformation( KFbxMesh *mesh, KTime &time, KFbxVector4 *vertices, KFbxPose *pose ) {
KFbxCluster::ELinkMode cluster_mode = ((KFbxSkin*)mesh->GetDeformer(0, KFbxDeformer::eSKIN))->GetCluster(0)->GetLinkMode();
int cp_count = mesh->GetControlPointsCount();
KFbxXMatrix *cluster_deformations = new KFbxXMatrix[cp_count];
memset( cluster_deformations, 0, cp_count * sizeof( KFbxXMatrix ) );
double *cluster_weights = new double[cp_count];
memset( cluster_weights, 0, cp_count * sizeof( double ) );
if( cluster_mode == KFbxCluster::eADDITIVE ) {
for( int i = 0;i < cp_count; i++ ) {
cluster_deformations[i].SetIdentity();
}
}
int skin_count = mesh->GetDeformerCount(KFbxDeformer::eSKIN);
for( int skin_index = 0; skin_index < skin_count; skin_index++ ) {
KFbxSkin *skin_deformer = (KFbxSkin*)mesh->GetDeformer(skin_index,KFbxDeformer::eSKIN);
int cluster_count = skin_deformer->GetClusterCount();
for( int cluster_index = 0; cluster_index < cluster_count; cluster_index++ ) {
KFbxCluster *cluster = skin_deformer->GetCluster(cluster_index);
if( !cluster->GetLink() ) continue;
KFbxXMatrix vertex_transform_matrix;
ComputeClusterDeformation( mesh, cluster, vertex_transform_matrix, time, pose );
int vertex_index_count = cluster->GetControlPointIndicesCount();
for( int k = 0; k < vertex_index_count; k++ ) {
int index = cluster->GetControlPointIndices()[k];
if( index >= cp_count ) continue;
double weight = cluster->GetControlPointWeights()[k];
if( weight == 0.0 ) continue;
KFbxXMatrix influence = vertex_transform_matrix;
MatrixScale( influence, weight );
if( cluster_mode == KFbxCluster::eADDITIVE ) {
MatrixAddToDiagonal( influence, 1.0 - weight );
cluster_deformations[index] = influence * cluster_deformations[index];
cluster_weights[index] = 1.0;
} else { // linkmode == normalize or total1
MatrixAdd( cluster_deformations[index], influence );
cluster_weights[index] += weight;
}
}
}
}
// actual deformation
for( int i = 0; i < cp_count; i++ ) {
KFbxVector4 source_vertex = vertices[i];
KFbxVector4 &dest_vertex = vertices[i];
double weight = cluster_weights[i];
if( weight != 0.0 ) {
dest_vertex = cluster_deformations[i].MultT( source_vertex );
if( cluster_mode == KFbxCluster::eNORMALIZE ) {
dest_vertex /= weight;
} else if( cluster_mode == KFbxCluster::eTOTAL1 ) {
source_vertex *= (1.0 - weight);
dest_vertex += source_vertex;
}
}
}
delete[] cluster_deformations;
delete[] cluster_weights;
}
static int
compute_cluster_statistics(MRI_SURFACE *mris, MRI *mri_profiles, MATRIX **m_covs, VECTOR **v_means, int k) {
int i, vno, cluster, nsamples, num[MAX_CLUSTERS];
int singular, cno_pooled, cno ;
MATRIX *m1, *mpooled, *m_inv_covs[MAX_CLUSTERS] ;
VECTOR *v1 ;
FILE *fp ;
double det, det_pooled ;
memset(num, 0, sizeof(num)) ;
nsamples = mri_profiles->nframes ;
v1 = VectorAlloc(nsamples, MATRIX_REAL) ;
m1 = MatrixAlloc(nsamples, nsamples, MATRIX_REAL) ;
mpooled = MatrixAlloc(nsamples, nsamples, MATRIX_REAL) ;
for (cluster = 0 ; cluster < k ; cluster++) {
VectorClear(v_means[cluster]) ;
MatrixClear(m_covs[cluster]) ;
}
// compute means
// fp = fopen("co.dat", "w") ;
fp = NULL ;
for (vno = 0 ; vno < mris->nvertices ; vno++) {
cluster = mris->vertices[vno].curv ;
for (i = 0 ; i < nsamples ; i++) {
VECTOR_ELT(v1, i+1) = MRIgetVoxVal(mri_profiles, vno, 0, 0, i) ;
if (cluster == 0 && fp)
fprintf(fp, "%f ", VECTOR_ELT(v1, i+1));
}
if (cluster == 0 && fp)
fprintf(fp, "\n") ;
num[cluster]++ ;
VectorAdd(v_means[cluster], v1, v_means[cluster]) ;
}
if (fp)
fclose(fp) ;
for (cluster = 0 ; cluster < k ; cluster++)
if (num[cluster] > 0)
VectorScalarMul(v_means[cluster], 1.0/(double)num[cluster], v_means[cluster]) ;
// compute inverse covariances
for (vno = 0 ; vno < mris->nvertices ; vno++) {
cluster = mris->vertices[vno].curv ;
for (i = 0 ; i < nsamples ; i++)
VECTOR_ELT(v1, i+1) = MRIgetVoxVal(mri_profiles, vno, 0, 0, i) ;
VectorSubtract(v_means[cluster], v1, v1) ;
VectorOuterProduct(v1, v1, m1) ;
MatrixAdd(m_covs[cluster], m1, m_covs[cluster]) ;
MatrixAdd(mpooled, m1, mpooled) ;
}
MatrixScalarMul(mpooled, 1.0/(double)mris->nvertices, mpooled) ;
cno_pooled = MatrixConditionNumber(mpooled) ;
det_pooled = MatrixDeterminant(mpooled) ;
for (cluster = 0 ; cluster < k ; cluster++)
if (num[cluster] > 0)
MatrixScalarMul(m_covs[cluster], 1.0/(double)num[cluster], m_covs[cluster]) ;
// invert all the covariance matrices
MatrixFree(&m1) ;
singular = 0 ;
for (cluster = 0 ; cluster < k ; cluster++) {
m1 = MatrixInverse(m_covs[cluster], NULL) ;
cno = MatrixConditionNumber(m_covs[cluster]) ;
det = MatrixDeterminant(m_covs[cluster]) ;
if (m1 == NULL)
singular++ ;
while (cno > 100*cno_pooled || 100*det < det_pooled) {
if (m1)
MatrixFree(&m1) ;
m1 = MatrixScalarMul(mpooled, 0.1, NULL) ;
MatrixAdd(m_covs[cluster], m1, m_covs[cluster]) ;
MatrixFree(&m1) ;
cno = MatrixConditionNumber(m_covs[cluster]) ;
m1 = MatrixInverse(m_covs[cluster], NULL) ;
det = MatrixDeterminant(m_covs[cluster]) ;
}
m_inv_covs[cluster] = m1 ;
}
for (cluster = 0 ; cluster < k ; cluster++) {
if (m_inv_covs[cluster] == NULL)
DiagBreak() ;
else {
MatrixFree(&m_covs[cluster]) ;
m_covs[cluster] = m_inv_covs[cluster] ;
// MatrixIdentity(m_covs[cluster]->rows, m_covs[cluster]);
}
}
MatrixFree(&mpooled) ;
VectorFree(&v1) ;
return(NO_ERROR) ;
}
// Deform the vertex array in classic linear way.
void ofxFBXMesh::computeLinearDeformation(FbxAMatrix& pGlobalPosition,
FbxMesh* pMesh,
FbxTime& pTime,
FbxVector4* pVertexArray,
FbxPose* pPose ) {
// All the links must have the same link mode.
FbxCluster::ELinkMode lClusterMode = ((FbxSkin*)fbxMesh->GetDeformer(0, FbxDeformer::eSkin))->GetCluster(0)->GetLinkMode();
int lVertexCount = pMesh->GetControlPointsCount();
// cout << "control points count = " << lVertexCount << " mesh verts = " << mesh.getNumVertices() << endl;
FbxAMatrix* lClusterDeformation = new FbxAMatrix[lVertexCount];
memset(lClusterDeformation, 0, lVertexCount * sizeof(FbxAMatrix));
double* lClusterWeight = new double[lVertexCount];
memset(lClusterWeight, 0, lVertexCount * sizeof(double));
if (lClusterMode == FbxCluster::eAdditive) {
for (int i = 0; i < lVertexCount; ++i) {
lClusterDeformation[i].SetIdentity();
}
}
// For all skins and all clusters, accumulate their deformation and weight
// on each vertices and store them in lClusterDeformation and lClusterWeight.
int lSkinCount = pMesh->GetDeformerCount(FbxDeformer::eSkin);
// cout << "computeLinearDeformation :: number of skins = " << lSkinCount << endl;
for ( int lSkinIndex=0; lSkinIndex<lSkinCount; ++lSkinIndex) {
FbxSkin * lSkinDeformer = (FbxSkin *)pMesh->GetDeformer(lSkinIndex, FbxDeformer::eSkin);
int lClusterCount = lSkinDeformer->GetClusterCount();
for ( int lClusterIndex=0; lClusterIndex<lClusterCount; ++lClusterIndex) {
FbxCluster* lCluster = lSkinDeformer->GetCluster(lClusterIndex);
if (!lCluster->GetLink())
continue;
FbxAMatrix lVertexTransformMatrix;
computeClusterDeformation(pGlobalPosition, pMesh, lCluster, lVertexTransformMatrix, pTime, pPose);
int lVertexIndexCount = lCluster->GetControlPointIndicesCount();
for (int k = 0; k < lVertexIndexCount; ++k) {
int lIndex = lCluster->GetControlPointIndices()[k];
// Sometimes, the mesh can have less points than at the time of the skinning
// because a smooth operator was active when skinning but has been deactivated during export.
if (lIndex >= lVertexCount)
continue;
double lWeight = lCluster->GetControlPointWeights()[k];
if (lWeight == 0.0) {
continue;
}
// Compute the influence of the link on the vertex.
FbxAMatrix lInfluence = lVertexTransformMatrix;
MatrixScale(lInfluence, lWeight);
if (lClusterMode == FbxCluster::eAdditive) {
// cout << "computeLinearDeformation :: clustermode = eAdditive" << endl;
// Multiply with the product of the deformations on the vertex.
MatrixAddToDiagonal(lInfluence, 1.0 - lWeight);
lClusterDeformation[lIndex] = lInfluence * lClusterDeformation[lIndex];
// Set the link to 1.0 just to know this vertex is influenced by a link.
lClusterWeight[lIndex] = 1.0;
} else // lLinkMode == FbxCluster::eNormalize || lLinkMode == FbxCluster::eTotalOne
{
// if(k == 0) cout << "computeLinearDeformation :: clustermode != eAdditive" << endl;
// Add to the sum of the deformations on the vertex.
MatrixAdd(lClusterDeformation[lIndex], lInfluence);
// Add to the sum of weights to either normalize or complete the vertex.
lClusterWeight[lIndex] += lWeight;
}
}//For each vertex
}//lClusterCount
}
//Actually deform each vertices here by information stored in lClusterDeformation and lClusterWeight
for (int i = 0; i < lVertexCount; i++) {
FbxVector4 lSrcVertex = pVertexArray[i];
FbxVector4& lDstVertex = pVertexArray[i];
double lWeight = lClusterWeight[i];
// Deform the vertex if there was at least a link with an influence on the vertex,
if (lWeight != 0.0) {
lDstVertex = lClusterDeformation[i].MultT(lSrcVertex);
if (lClusterMode == FbxCluster::eNormalize) {
// In the normalized link mode, a vertex is always totally influenced by the links.
lDstVertex /= lWeight;
} else if (lClusterMode == FbxCluster::eTotalOne) {
// In the total 1 link mode, a vertex can be partially influenced by the links.
lSrcVertex *= (1.0 - lWeight);
lDstVertex += lSrcVertex;
}
}
}
delete [] lClusterDeformation;
delete [] lClusterWeight;
}
int
main(int argc, char *argv[]) {
char **av, *cp ;
int ac, nargs, i, dof, no_transform, which, sno = 0, nsubjects = 0 ;
MRI *mri=0, *mri_mean = NULL, *mri_std=0, *mri_T1=0,*mri_binary=0,*mri_dof=NULL,
*mri_priors = NULL ;
char *subject_name, *out_fname, fname[STRLEN] ;
/* LTA *lta;*/
MRI *mri_tmp=0 ;
/* rkt: check for and handle version tag */
nargs = handle_version_option (argc, argv, "$Id: mri_make_template.c,v 1.26 2011/03/02 00:04:22 nicks Exp $", "$Name: stable5 $");
if (nargs && argc - nargs == 1)
exit (0);
argc -= nargs;
Progname = argv[0] ;
ErrorInit(NULL, NULL, NULL) ;
DiagInit(NULL, NULL, NULL) ;
ac = argc ;
av = argv ;
for ( ; argc > 1 && ISOPTION(*argv[1]) ; argc--, argv++) {
nargs = get_option(argc, argv) ;
argc -= nargs ;
argv += nargs ;
}
if (!strlen(subjects_dir)) {
cp = getenv("SUBJECTS_DIR") ;
if (!cp)
ErrorExit(ERROR_BADPARM,"%s: SUBJECTS_DIR not defined in environment.\n",
Progname) ;
strcpy(subjects_dir, cp) ;
}
if (argc < 3) usage_exit(1) ;
out_fname = argv[argc-1] ;
no_transform = first_transform ;
if (binary_name) /* generate binarized volume with priors and */
{ /* separate means and variances */
for (which = BUILD_PRIORS ; which <= OFF_STATS ; which++) {
/* for each subject specified on cmd line */
for (dof = 0, i = 1 ; i < argc-1 ; i++) {
if (*argv[i] == '-') /* don't do transform for next subject */
{ no_transform = 1 ;
continue ;
}
dof++ ;
subject_name = argv[i] ;
if (which != BUILD_PRIORS) {
sprintf(fname, "%s/%s/mri/%s", subjects_dir, subject_name, T1_name);
fprintf(stderr, "%d of %d: reading %s...\n", i, argc-2, fname) ;
mri_T1 = MRIread(fname) ;
if (!mri_T1)
ErrorExit(ERROR_NOFILE,"%s: could not open volume %s",
Progname,fname);
}
sprintf(fname, "%s/%s/mri/%s",subjects_dir,subject_name,binary_name);
fprintf(stderr, "%d of %d: reading %s...\n", i, argc-2, fname) ;
mri_binary = MRIread(fname) ;
if (!mri_binary)
ErrorExit(ERROR_NOFILE,"%s: could not open volume %s",
Progname,fname);
/* only count voxels which are mostly labeled */
MRIbinarize(mri_binary, mri_binary, WM_MIN_VAL, 0, 100) ;
if (transform_fname && no_transform-- <= 0) {
sprintf(fname, "%s/%s/mri/transforms/%s",
subjects_dir, subject_name, transform_fname) ;
fprintf(stderr, "reading transform %s...\n", fname) ;
////////////////////////////////////////////////////////
#if 1
{
TRANSFORM *transform ;
transform = TransformRead(fname) ;
if (transform == NULL)
ErrorExit(ERROR_NOFILE, "%s: could not open transform file %s\n",Progname, fname) ;
mri_tmp = TransformApply(transform, mri_T1, NULL) ;
TransformFree(&transform) ;
}
#else
lta = LTAreadEx(fname);
if (lta == NULL)
ErrorExit(ERROR_NOFILE,
"%s: could not open transform file %s\n",
Progname, fname) ;
/* LTAtransform() runs either MRIapplyRASlinearTransform()
for RAS2RAS or MRIlinearTransform() for Vox2Vox. */
/* MRIlinearTransform() calls MRIlinearTransformInterp() */
mri_tmp = LTAtransform(mri_T1, NULL, lta);
MRIfree(&mri_T1) ;
mri_T1 = mri_tmp ;
LTAfree(<a);
lta = NULL;
#endif
if (DIAG_VERBOSE_ON)
fprintf(stderr, "transform application complete.\n") ;
}
if (which == BUILD_PRIORS) {
mri_priors =
MRIupdatePriors(mri_binary, mri_priors) ;
} else {
if (!mri_mean) {
mri_dof = MRIalloc(mri_T1->width, mri_T1->height, mri_T1->depth,
MRI_UCHAR) ;
mri_mean =
MRIalloc(mri_T1->width, mri_T1->height,mri_T1->depth,MRI_FLOAT);
mri_std =
MRIalloc(mri_T1->width,mri_T1->height,mri_T1->depth,MRI_FLOAT);
if (!mri_mean || !mri_std)
ErrorExit(ERROR_NOMEMORY, "%s: could not allocate templates.\n",
Progname) ;
}
if (DIAG_VERBOSE_ON)
fprintf(stderr, "updating mean and variance estimates...\n") ;
if (which == ON_STATS) {
MRIaccumulateMaskedMeansAndVariances(mri_T1, mri_binary, mri_dof,
90, 100, mri_mean, mri_std) ;
fprintf(stderr, "T1 = %d, binary = %d, mean = %2.1f\n",
(int)MRIgetVoxVal(mri_T1, 141,100,127,0),
MRIvox(mri_binary, 141,100,127),
MRIFvox(mri_mean, 141,100,127)) ;
} else /* computing means and vars for off */
MRIaccumulateMaskedMeansAndVariances(mri_T1, mri_binary, mri_dof,
0, WM_MIN_VAL-1,
mri_mean, mri_std) ;
MRIfree(&mri_T1) ;
}
MRIfree(&mri_binary) ;
}
if (which == BUILD_PRIORS) {
mri = MRIcomputePriors(mri_priors, dof, NULL) ;
MRIfree(&mri_priors) ;
fprintf(stderr, "writing priors to %s...\n", out_fname) ;
} else {
MRIcomputeMaskedMeansAndStds(mri_mean, mri_std, mri_dof) ;
mri_mean->dof = dof ;
fprintf(stderr, "writing T1 means with %d dof to %s...\n", mri_mean->dof,
out_fname) ;
if (!which)
MRIwrite(mri_mean, out_fname) ;
else
MRIappend(mri_mean, out_fname) ;
MRIfree(&mri_mean) ;
fprintf(stderr, "writing T1 variances to %s...\n", out_fname);
if (dof <= 1)
MRIreplaceValues(mri_std, mri_std, 0, 1) ;
mri = mri_std ;
}
if (!which)
MRIwrite(mri, out_fname) ;
else
MRIappend(mri, out_fname) ;
MRIfree(&mri) ;
}
}
else {
/* for each subject specified on cmd line */
if (xform_mean_fname) {
m_xform_mean = MatrixAlloc(4,4,MATRIX_REAL) ;
/* m_xform_covariance = MatrixAlloc(12,12,MATRIX_REAL) ;*/
}
dof = 0;
for (i = 1 ; i < argc-1 ; i++) {
if (*argv[i] == '-') {
/* don't do transform for next subject */
no_transform = 1 ;
continue ;
}
dof++ ;
subject_name = argv[i] ;
sprintf(fname, "%s/%s/mri/%s", subjects_dir, subject_name, T1_name);
fprintf(stderr, "%d of %d: reading %s...\n", i, argc-2, fname) ;
mri_T1 = MRIread(fname) ;
if (!mri_T1)
ErrorExit(ERROR_NOFILE,"%s: could not open volume %s",Progname,fname);
check_mri(mri_T1) ;
if (binarize)
MRIbinarize(mri_T1, mri_T1, binarize, 0, 1) ;
if (erode) {
int i ;
printf("eroding input %d times\n", erode) ;
for (i = 0 ; i < erode ; i++)
MRIerode(mri_T1, mri_T1) ;
}
if (open) {
int i ;
printf("opening input %d times\n", open) ;
for (i = 0 ; i < open ; i++)
MRIerode(mri_T1, mri_T1) ;
for (i = 0 ; i < open ; i++)
MRIdilate(mri_T1, mri_T1) ;
}
check_mri(mri_T1) ;
if (transform_fname) {
sprintf(fname, "%s/%s/mri/transforms/%s",
subjects_dir, subject_name, transform_fname) ;
fprintf(stderr, "reading transform %s...\n", fname) ;
////////////////////////////////////////////////////////
#if 1
{
TRANSFORM *transform ;
transform = TransformRead(fname) ;
if (transform == NULL)
ErrorExit(ERROR_NOFILE, "%s: could not open transform file %s\n",Progname, fname) ;
mri_tmp = TransformApply(transform, mri_T1, NULL) ;
if (DIAG_VERBOSE_ON)
MRIwrite(mri_tmp, "t1.mgz") ;
TransformFree(&transform) ;
}
#else
lta = LTAreadEx(fname);
if (lta == NULL)
ErrorExit(ERROR_NOFILE,
"%s: could not open transform file %s\n",
Progname, fname) ;
printf("transform matrix -----------------------\n");
MatrixPrint(stdout,lta->xforms[0].m_L);
/* LTAtransform() runs either MRIapplyRASlinearTransform()
for RAS2RAS or MRIlinearTransform() for Vox2Vox. */
/* MRIlinearTransform() calls MRIlinearTransformInterp() */
mri_tmp = LTAtransform(mri_T1, NULL, lta);
printf("----- -----------------------\n");
LTAfree(<a);
#endif
MRIfree(&mri_T1);
mri_T1 = mri_tmp ; // reassign pointers
if (DIAG_VERBOSE_ON)
fprintf(stderr, "transform application complete.\n") ;
}
if (!mri_mean) {
mri_mean =
MRIalloc(mri_T1->width, mri_T1->height, mri_T1->depth, MRI_FLOAT) ;
mri_std =
MRIalloc(mri_T1->width, mri_T1->height, mri_T1->depth, MRI_FLOAT) ;
if (!mri_mean || !mri_std)
ErrorExit(ERROR_NOMEMORY, "%s: could not allocate templates.\n",
Progname) ;
// if(transform_fname == NULL){
if (DIAG_VERBOSE_ON)
printf("Copying geometry\n");
MRIcopyHeader(mri_T1,mri_mean);
MRIcopyHeader(mri_T1,mri_std);
// }
}
check_mri(mri_mean) ;
if (!stats_only) {
if (DIAG_VERBOSE_ON)
fprintf(stderr, "updating mean and variance estimates...\n") ;
MRIaccumulateMeansAndVariances(mri_T1, mri_mean, mri_std) ;
}
check_mri(mri_mean) ;
if (DIAG_VERBOSE_ON)
MRIwrite(mri_mean, "t2.mgz") ;
MRIfree(&mri_T1) ;
no_transform = 0;
} /* end loop over subjects */
if (xform_mean_fname) {
FILE *fp ;
VECTOR *v = NULL, *vT = NULL ;
MATRIX *m_vvT = NULL ;
int rows, cols ;
nsubjects = sno ;
fp = fopen(xform_covariance_fname, "w") ;
if (!fp)
ErrorExit(ERROR_NOFILE, "%s: could not open covariance file %s",
Progname, xform_covariance_fname) ;
fprintf(fp, "nsubjects=%d\n", nsubjects) ;
MatrixScalarMul(m_xform_mean, 1.0/(double)nsubjects, m_xform_mean) ;
printf("means:\n") ;
MatrixPrint(stdout, m_xform_mean) ;
MatrixAsciiWrite(xform_mean_fname, m_xform_mean) ;
/* subtract the mean from each transform */
rows = m_xform_mean->rows ;
cols = m_xform_mean->cols ;
for (sno = 0 ; sno < nsubjects ; sno++) {
MatrixSubtract(m_xforms[sno], m_xform_mean, m_xforms[sno]) ;
v = MatrixReshape(m_xforms[sno], v, rows*cols, 1) ;
vT = MatrixTranspose(v, vT) ;
m_vvT = MatrixMultiply(v, vT, m_vvT) ;
if (!m_xform_covariance)
m_xform_covariance =
MatrixAlloc(m_vvT->rows, m_vvT->cols,MATRIX_REAL) ;
MatrixAdd(m_vvT, m_xform_covariance, m_xform_covariance) ;
MatrixAsciiWriteInto(fp, m_xforms[sno]) ;
}
MatrixScalarMul(m_xform_covariance, 1.0/(double)nsubjects,
m_xform_covariance) ;
printf("covariance:\n") ;
MatrixPrint(stdout, m_xform_covariance) ;
MatrixAsciiWriteInto(fp, m_xform_covariance) ;
fclose(fp) ;
if (stats_only)
exit(0) ;
}
MRIcomputeMeansAndStds(mri_mean, mri_std, dof) ;
check_mri(mri_mean) ;
check_mri(mri_std) ;
mri_mean->dof = dof ;
if (smooth) {
MRI *mri_kernel, *mri_smooth ;
printf("applying smoothing kernel\n") ;
mri_kernel = MRIgaussian1d(smooth, 100) ;
mri_smooth = MRIconvolveGaussian(mri_mean, NULL, mri_kernel) ;
MRIfree(&mri_kernel) ;
MRIfree(&mri_mean) ;
mri_mean = mri_smooth ;
}
fprintf(stderr, "\nwriting T1 means with %d dof to %s...\n", mri_mean->dof,
out_fname) ;
MRIwrite(mri_mean, out_fname) ;
MRIfree(&mri_mean) ;
if (dof <= 1) /* can't calculate variances - set them to reasonable val */
{
// src dst
MRIreplaceValues(mri_std, mri_std, 0, 1) ;
}
if (!novar) {
// mri_std contains the variance here (does it?? I don't think so -- BRF)
if (!var_fname) {
fprintf(stderr, "\nwriting T1 standard deviations to %s...\n", out_fname);
MRIappend(mri_std, out_fname) ;
} else {
fprintf(stderr, "\nwriting T1 standard deviations to %s...\n", var_fname);
MRIwrite(mri_std, var_fname) ;
}
}
MRIfree(&mri_std) ;
if (mri)
MRIfree(&mri);
} /* end if binarize */
return(0) ;
}
int main(void)
{
char mStr[1024];
matrix *ans, *m1, *m2;
int sc = 0;
printf("Which operation: ");
char op = tolower(getchar());
while(op != '+' || op != '-' || op != '*' || op != '/' || op != 'i') {
puts(opErr);
op = tolower(getchar());
}
printf("First matrix:\n");
scanf("%s", mStr);
m1 = MatrixInit(mStr);
MatrixPrint(m1);
if(op == 'a' || op == 's' || op == 'm') {
printf("Second matrix:\n");
scanf("%s", mStr);
m2 = MatrixInit(mStr);
MatrixPrint(m2);
} else if(op == 'c') {
printf("Scalar multiple:\n");
scanf("%d", &sc);
}
switch(op) {
case 'a':
ans = MatrixAdd(m1, m2);
break;
case 's':
ans = MatrixSub(m1, m2);
break;
case 'm':
ans = MatrixMul(m1, m2);
break;
case 'i':
ans = MatrixInv(m1);
break;
case 'c':
ans = MatrixSMul(m1, sc);
break;
default:
printf("Something went very wrong.\n");
return 1;
}
printf("Answer:\n");
MatrixPrint(ans);
MatrixFree(m1);
MatrixFree(ans);
if(op == 'a' || op == 's' || op == 'm') {
MatrixFree(m2);
}
return 0;
}
