int main()
{
int a[X][Y] = {
2,-6,
3,5,
1,-1
};
int b[Y][Z] = {
4,-2,-4,-5,
-7,-3,6,7
};
int c[X][Z];
matrix_multiply( &a[0][0], &b[0][0], &c[0][0],X,Y,Z);
pMatrix(&c[0][0],X,Z);
return EXIT_SUCCESS;
}
void dagPoseInfo::printDagPoseInfo(MObject& dagPoseNode, unsigned index)
//
// Description:
// Given a dagPose and an index corresponding to a joint, print out
// the matrix info for the joint.
// Return:
// None.
//
{
MFnDependencyNode nDagPose(dagPoseNode);
fprintf(file,"%s\n",nDagPose.name().asChar());
// construct plugs for this joints world and local matrices
//
MObject aWorldMatrix = nDagPose.attribute("worldMatrix");
MPlug pWorldMatrix(dagPoseNode,aWorldMatrix);
pWorldMatrix.selectAncestorLogicalIndex(index,aWorldMatrix);
MObject aMatrix = nDagPose.attribute("xformMatrix");
MPlug pMatrix(dagPoseNode,aMatrix);
pMatrix.selectAncestorLogicalIndex(index,aMatrix);
// get and print the world matrix data
//
MObject worldMatrix, xformMatrix;
MStatus status = pWorldMatrix.getValue(worldMatrix);
if (MS::kSuccess != status) {
displayError("Problem retrieving world matrix.");
} else {
bool foundMatrix = 0;
MFnMatrixData dMatrix(worldMatrix);
MMatrix wMatrix = dMatrix.matrix(&status);
if (MS::kSuccess == status) {
foundMatrix = 1;
unsigned jj,kk;
fprintf(file,"worldMatrix\n");
for (jj = 0; jj < 4; ++jj) {
for (kk = 0; kk < 4; ++kk) {
double val = wMatrix(jj,kk);
fprintf(file,"%f ",val);
}
fprintf(file,"\n");
}
}
if (!foundMatrix) {
displayError("Error getting world matrix data.");
}
}
// get and print the local matrix data
//
status = pMatrix.getValue(xformMatrix);
if (MS::kSuccess != status) {
displayError("Problem retrieving xform matrix.");
} else {
bool foundMatrix = 0;
MFnMatrixData dMatrix(xformMatrix);
if (dMatrix.isTransformation()) {
MTransformationMatrix xform = dMatrix.transformation(&status);
if (MS::kSuccess == status) {
foundMatrix = 1;
MMatrix xformAsMatrix = xform.asMatrix();
unsigned jj,kk;
fprintf(file,"matrix\n");
for (jj = 0; jj < 4; ++jj) {
for (kk = 0; kk < 4; ++kk) {
double val = xformAsMatrix(jj,kk);
fprintf(file,"%f ",val);
}
fprintf(file,"\n");
}
}
}
if (!foundMatrix) {
displayError("Error getting local matrix data.");
}
}
}
/**
* calcCLG_OF
*
* Main CLG-optical flow (CLG-OF) computation function.
*
* Parameters:
*
* image1 Pointer to the first image (the "previous" time frame).
* image2 Pointer to the second image (the "current" time frame).
* uOut Pointer to the horizontal component of the CLG-OF solution.
* vOut Pointer to the vertical component of the CLG-OF solution.
* nRows Number of image rows (same for the CLG-OF vector field).
* nCols Number of image columns (same for the CLG-OF vector field).
* iterations Number of iterations for iterative solution.
* alpha Global smoothing coefficient of the CLG-OF.
* rho Local spatio-temporal smoothing coefficient of the CLG-OF.
* wFactor SOR relaxation factor, between 0 and 2.
* verbose Display/hide messages to the stdout.
* coupledMode Iteration type. 1->Pointwise-Coupled Gauss-Seidel, 0->SOR.
*
*/
int calcCLG_OF(double* image1,
double* image2,
double* uOut,
double* vOut,
int nRows,
int nCols,
int iterations,
double alpha,
double rho,
double wFactor,
int verbose,
int coupledMode) {
if (verbose) {
printf("calc_clg\n");
printf(" setting up variables\n");
}
int i=0, j=0;
// h, could be less than 1.0.
double h = 1.0;
// Matrix to vector.
double **prevFrame, **currFrame;
prevFrame = pMatrix(nRows, nCols);
currFrame = pMatrix(nRows, nCols);
for (i=0; i<nRows; i++) {
for (j=0; j<nCols; j++) {
prevFrame[i][j] = image1[j + i*nCols];
currFrame[i][j] = image2[j + i*nCols];
}
}
if (verbose)
printf(" allocating memory for arrays\n");
double **u, **v;
double **dfdx, **dfdxw;
double **dfdy, **dfdyw;
double **dfdt, **dfdtw;
u = pMatrix(nRows, nCols);
v = pMatrix(nRows, nCols);
// Derivatives and their warped versions.
dfdx = pMatrix(nRows, nCols);
dfdy = pMatrix(nRows, nCols);
dfdt = pMatrix(nRows, nCols);
for (i=0; i<nRows; i++) {
for (j=0; j<nCols; j++) {
u[i][j] = 0.0;
v[i][j] = 0.0;
dfdt[i][j] = currFrame[i][j] - prevFrame[i][j];
}
}
if (verbose)
printf(" allocating memory for derivatives matrices\n");
double **J[JROWS][JCOLS];
// Because of symmetry, only the upper part is allocated.
int k, l;
for (k=0; k<JROWS; k++)
for (l=k; l<JCOLS; l++)
J[k][l] = pMatrix(nRows, nCols);
// Spatial derivatives obtention.
computeDerivatives(prevFrame, currFrame, dfdx, dfdy, nRows, nCols, verbose);
// Compute J tensor.
computeJTensor(dfdx, dfdy, dfdt, J, nRows, nCols);
if (verbose)
printf(" local spatio temporal smoothing\n");
if (rho > 0) {
k=0, l=0;
for (k=0; k<JROWS; k++)
for (l=k; l<JCOLS; l++)
matrixSmooth(J[k][l], nRows, nCols, rho);
}
if (iterations == 0)
iterations = (int) (nRows * nCols / 8.0);
if (verbose)
printf(" performing %i relax iterations\n", iterations);
int count = 0;
double error = 1000000;
double convergenceError = 0.0;
for (count=0; count<iterations && convergenceError*1.01 < error; count++) {
if (count > 0)
error = convergenceError;
if (coupledMode == 1) {
if (verbose && count % 50 == 0 && count > 0)
printf(" iteration %d/%d (P-C Gauss-Seidel), error=%f\n",
count, iterations, error);
convergenceError = relaxPointwiseCoupledGaussSeidel(u, v, J,
nRows, nCols,
alpha);
} else {
if (verbose && count % 50 == 0 && count > 0)
printf(" iteration %d/%d (SOR), error=%f\n",
count, iterations, error);
convergenceError = relaxSOR(u, v, J, nRows, nCols, alpha, wFactor);
}
}
// Show debug information.
if (verbose)
printf(" filling output after %d iterations, error=%f\n", count, error);
// Fill output variables.
for (i=0; i<nRows; i++) {
for (j=0; j<nCols; j++) {
uOut[j + i*nCols] += u[i][j];
vOut[j + i*nCols] += v[i][j];
}
}
// Free memory.
if (verbose)
printf(" freeing memory\n");
freePmatrix(u, nRows);
freePmatrix(v, nRows);
for (k=0; k<JROWS; k++)
for (l=k; l<JCOLS; l++)
freePmatrix(J[k][l], nRows);
freePmatrix(prevFrame, nRows);
freePmatrix(currFrame, nRows);
freePmatrix(dfdx, nRows);
freePmatrix(dfdy, nRows);
freePmatrix(dfdt, nRows);
if (verbose)
printf("calc_clg: done\n");
return 1;
}
