double gmmreg_tps_func::f(const vnl_vector<double>& x) {
gmmreg->perform_transform(x);
double bending = gmmreg->bending_energy();
double energy1 = GaussTransform(gmmreg->transformed_model,
gmmreg->transformed_model, scale, gradient1);
double energy2 = GaussTransform(gmmreg->transformed_model,
gmmreg->scene, scale, gradient2);
double energy3 = GaussTransform(gmmreg->scene, gmmreg->scene, scale);
double energy = eval(energy1,energy2, gradient1, gradient2);
energy = energy1 - 2*energy2 + energy3;
energy += lambda*bending;
// std::cout<<"bending : "<<bending<<std::endl;
// std::cout<<"energy1 : "<<energy1<<std::endl;
// std::cout<<"energy2 : "<<energy2<<std::endl;
// std::cout<<"lambda : "<<lambda<<std::endl;
// std::cout<<"energy : "<<energy<<std::endl;
// publish();
return energy;
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* Declare variables */
int m, n, dim;
double *A, *B, *result, *grad, scale;
/* Check for proper number of input and output arguments */
if (nrhs != 3) {
mexErrMsgTxt("Three input arguments required.");
}
if (nlhs > 2){
mexErrMsgTxt("Too many output arguments.");
}
/* Check data type of input argument */
if (!(mxIsDouble(prhs[0]))) {
mexErrMsgTxt("Input array must be of type double.");
}
if (!(mxIsDouble(prhs[1]))) {
mexErrMsgTxt("Input array must be of type double.");
}
if (!(mxIsDouble(prhs[2]))) {
mexErrMsgTxt("Input array must be of type double.");
}
/* Get the number of elements in the input argument */
/* elements=mxGetNumberOfElements(prhs[0]); */
/* Get the data */
A = (double *)mxGetPr(prhs[0]);
B = (double *)mxGetPr(prhs[1]);
scale = mxGetScalar(prhs[2]);
/* Get the dimensions of the matrix input A&B. */
m = mxGetN(prhs[0]);
n = mxGetN(prhs[1]);
dim = mxGetM(prhs[0]);
if (mxGetM(prhs[1])!=dim)
{
mexErrMsgTxt("The two input point sets should have same dimension.");
}
/* Allocate the space for the return argument */
plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(dim,m,mxREAL);
result = mxGetPr(plhs[0]);
grad = mxGetPr(plhs[1]);
*result = GaussTransform(A, B, m, n, dim, scale, grad);
}
static PyObject *
py_gauss_transform(PyObject *self, PyObject *args)
{
int m,n,dim;
double scale, result;
double *grad;
PyObject *A, *B;
PyArrayObject *arrayA, *arrayB;
PyArrayObject *arrayGrad;
PyObject *list;
/* send Python arrays to C */
if (!PyArg_ParseTuple(args, "OOd", &A, &B, &scale))
{
return NULL;
}
/* printf("getting input success. \n"); */
arrayA = (PyArrayObject *) PyArray_ContiguousFromObject(A, PyArray_DOUBLE, 1, 2);
arrayB = (PyArrayObject *) PyArray_ContiguousFromObject(B, PyArray_DOUBLE, 1, 2);
/* printf("converting success!\n"); */
if (arrayA->nd > 2 || arrayA->descr->type_num != PyArray_DOUBLE) {
PyErr_SetString(PyExc_ValueError,
"array must be two-dimensional and of type float");
return NULL;
}
/* printf("checking input success!\n"); */
m = (arrayA->dimensions)[0];
n = (arrayB->dimensions)[0];
if (arrayA->nd>1)
dim = (arrayA->dimensions)[1];
else
dim = 1;
/* printf("m=%d,n=%d,dim=%d\n",m,n,dim); */
grad = (double *) malloc(m*dim*sizeof(double));
/* call function */
result = GaussTransform((double*)(arrayA->data), (double*)(arrayB->data), m, n, dim, scale, grad);
/* PyArray_FromDimsAndData() deprecated, use PyArray_SimpleNewFromData()
http://blog.enthought.com/?p=62
arrayGrad = (PyArrayObject*) PyArray_FromDimsAndData(2,arrayA->dimensions,PyArray_DOUBLE, (char*)grad);
*/
arrayGrad = PyArray_SimpleNewFromData(2,arrayA->dimensions,NPY_DOUBLE,grad);
if (arrayGrad == NULL){
printf("creating %dx%d array failed\n", arrayA->dimensions[0],arrayA->dimensions[1]);
return NULL;
}
/* free(grad); */
/* send the result back to Python */
Py_DECREF(arrayA);
Py_DECREF(arrayB);
//Build a list; send it back to interpreter
list = PyList_New(0);
// Check the API documentation for meaning of
// return values.
if(PyList_Append(list, PyFloat_FromDouble(result)) != 0)
{
// set exception context, raise (return 0)
return 0;
}
if(PyList_Append(list, PyArray_Return(arrayGrad)) != 0)
{
// set exception context, raise (return 0)
return 0;
}
return list;
//return PyArray_Return(arrayGrad);
//return PyFloat_FromDouble(result);
/*return Py_BuildValue("d", result);*/
}
double gmmreg_rigid_func::f(const vnl_vector<double>& x) {
gmmreg->perform_transform(x);
double energy = GaussTransform(gmmreg->transformed_model,
gmmreg->scene, scale, gradient);
return eval(energy, gradient);
}
double GaussTransform(const vnl_matrix<double>& A,
const vnl_matrix<double>& B, double scale) {
// assert A.cols() == B.cols()
return GaussTransform(A.data_block(), B.data_block(),
A.rows(), B.rows(), A.cols(), scale);
}
