static PyObject *
complex_add(PyObject *v, PyObject *w)
{
Py_complex result;
Py_complex a, b;
TO_COMPLEX(v, a);
TO_COMPLEX(w, b);
PyFPE_START_PROTECT("complex_add", return 0)
result = c_sum(a, b);
PyFPE_END_PROTECT(result)
return PyComplex_FromCComplex(result);
}
unsigned int open_write_close(int int_fifo, char* char_fifo, char* path){
char buffer[MAX_BUFFER];
ssize_t size;
sprintf(buffer, "%d\n", c_sum(path));
if ( (int_fifo=open(char_fifo, O_WRONLY)) < 0 ){
perror("fifo open");
return EXIT_FAILURE;
}
size = write(int_fifo, buffer, sizeof(char)*(strlen(buffer)+1));
if( size != (strlen(buffer)+1) ){
printf("Can't write all string to FIFO\n");
return EXIT_FAILURE;
}
close(int_fifo);
return EXIT_SUCCESS;
}
void SFMViewer::update(std::vector<cv::Point3d> pcld,
std::vector<cv::Vec3b> pcldrgb,
std::vector<cv::Point3d> pcld_alternate,
std::vector<cv::Vec3b> pcldrgb_alternate,
std::vector<cv::Matx34d> cameras) {
m_pcld = pcld;
m_pcldrgb = pcldrgb;
m_cameras = cameras;
//get the scale of the result cloud using PCA
{
cv::Mat_<double> cldm(pcld.size(), 3);
for (unsigned int i = 0; i < pcld.size(); i++) {
cldm.row(i)(0) = pcld[i].x;
cldm.row(i)(1) = pcld[i].y;
cldm.row(i)(2) = pcld[i].z;
}
cv::Mat_<double> mean; //cv::reduce(cldm,mean,0,CV_REDUCE_AVG);
cv::PCA pca(cldm, mean, CV_PCA_DATA_AS_ROW);
scale_cameras_down = 1.0 / (3.0 * sqrt(pca.eigenvalues.at<double> (0)));
// std::cout << "emean " << mean << std::endl;
// m_global_transform = Eigen::Translation<double,3>(-Eigen::Map<Eigen::Vector3d>(mean[0]));
}
//compute transformation to place cameras in world
m_cameras_transforms.resize(m_cameras.size());
Eigen::Vector3d c_sum(0,0,0);
for (int i = 0; i < m_cameras.size(); ++i) {
Eigen::Matrix<double, 3, 4> P = Eigen::Map<Eigen::Matrix<double, 3, 4,
Eigen::RowMajor> >(m_cameras[i].val);
Eigen::Matrix3d R = P.block(0, 0, 3, 3);
Eigen::Vector3d t = P.block(0, 3, 3, 1);
Eigen::Vector3d c = -R.transpose() * t;
c_sum += c;
m_cameras_transforms[i] =
Eigen::Translation<double, 3>(c) *
Eigen::Quaterniond(R) *
Eigen::UniformScaling<double>(scale_cameras_down)
;
}
m_global_transform = Eigen::Translation<double,3>(-c_sum / (double)(m_cameras.size()));
// m_global_transform = m_cameras_transforms[0].inverse();
updateGL();
}
static Py_complex
c_atanh(Py_complex x)
{
return c_prod(c_half,c_log(c_quot(c_sum(c_one,x),c_diff(c_one,x))));
}
static Py_complex
c_acos(Py_complex x)
{
return c_neg(c_prodi(c_log(c_sum(x,c_prod(c_i,
c_sqrt(c_diff(c_one,c_prod(x,x))))))));
}
/*
* function [lp]=gaussmixp(y,m,v,w)
* y = cat(1, testSamples(1).mfcc{:});
* m = gmm.M;
* v = gmm.V;
* w = gmm.W;
*/
void gaussmixp(const real_T y[2004], const real_T m[108], const real_T v[108],
const real_T w[9], real_T lp[167])
{
real_T b[108];
real_T dv0[108];
real_T b_b[9];
real_T lvm[9];
int32_T ix;
real_T mx[167];
real_T kk[1503];
real_T km[1503];
static real_T b_y[18036];
int32_T iy;
real_T dv1[18036];
real_T x[1503];
int32_T i;
int32_T ixstart;
int32_T ixstop;
real_T mtmp;
int32_T b_ix;
boolean_T exitg1;
real_T ps[167];
/* GAUSSMIXP calculate probability densities from a Gaussian mixture model */
/* */
/* Inputs: n data values, k mixtures, p parameters, q data vector size */
/* */
/* Y(n,q) = input data */
/* M(k,p) = mixture means for x(p) */
/* V(k,p) or V(p,p,k) variances (diagonal or full) */
/* W(k,1) = weights */
/* A(q,p), B(q) = transformation: y=x*a'+b' (where y and x are row vectors) */
/* if A is omitted, it is assumed to be the first q rows of the */
/* identity matrix. B defaults to zero. */
/* Note that most commonly, q=p and A and B are omitted entirely. */
/* */
/* Outputs */
/* */
/* LP(n,1) = log probability of each data point */
/* RP(n,k) = relative probability of each mixture */
/* KH(n,1) = highest probability mixture */
/* KP(n,1) = relative probability of highest probability mixture */
/* Copyright (C) Mike Brookes 2000-2009 */
/* Version: $Id: gaussmixp.m,v 1.3 2009/04/08 07:51:21 dmb Exp $ */
/* */
/* VOICEBOX is a MATLAB toolbox for speech processing. */
/* Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html */
/* */
/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
/* This program is free software; you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; either version 2 of the License, or */
/* (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You can obtain a copy of the GNU General Public License from */
/* http://www.gnu.org/copyleft/gpl.html or by writing to */
/* Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. */
/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
/* 'gaussmixp:49' [n,q]=size(y); */
/* 'gaussmixp:50' [k,p]=size(m); */
/* 'gaussmixp:51' memsize=voicebox('memsize'); */
voicebox();
/* set memory size to use */
/* 'gaussmixp:53' lp=zeros(n,1); */
memset((void *)&lp[0], 0, 167U * sizeof(real_T));
/* 'gaussmixp:54' wk=ones(k,1); */
/* 'gaussmixp:56' vi=-0.5*v.^(-1); */
power(v, b);
/* data-independent scale factor in exponent */
/* 'gaussmixp:57' lvm=log(w)-0.5*sum(log(v),2); */
memcpy((void *)&dv0[0], (void *)&v[0], 108U * sizeof(real_T));
c_log(dv0);
sum(dv0, b_b);
memcpy((void *)&lvm[0], (void *)&w[0], 9U * sizeof(real_T));
b_log(lvm);
for (ix = 0; ix < 9; ix++) {
lvm[ix] -= 0.5 * b_b[ix];
}
/* log of external scale factor (excluding -0.5*q*log(2pi) term) */
/* 'gaussmixp:58' ii=1:n; */
/* 'gaussmixp:59' wnj=ones(1,n); */
/* 'gaussmixp:60' kk=repmat(ii,k,1); */
for (ix = 0; ix < 167; ix++) {
mx[ix] = 1.0 + (real_T)ix;
}
repmat(mx, kk);
/* 'gaussmixp:61' km=repmat(1:k,1,n); */
for (ix = 0; ix < 9; ix++) {
b_b[ix] = 1.0 + (real_T)ix;
}
b_repmat(b_b, km);
/* 'gaussmixp:62' py=reshape(sum((y(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,n)+lvm(:,wnj); */
for (ix = 0; ix < 12; ix++) {
for (iy = 0; iy < 1503; iy++) {
b_y[iy + 1503 * ix] = y[((int32_T)kk[iy] + 167 * ix) - 1] - m[((int32_T)
km[iy] + 9 * ix) - 1];
}
}
b_power(b_y, dv1);
for (ix = 0; ix < 12; ix++) {
for (iy = 0; iy < 1503; iy++) {
b_y[iy + 1503 * ix] = dv1[iy + 1503 * ix] * (-0.5 * b[((int32_T)km[iy] + 9
* ix) - 1]);
}
}
b_sum(b_y, x);
memcpy((void *)&kk[0], (void *)&x[0], 1503U * sizeof(real_T));
for (ix = 0; ix < 167; ix++) {
for (iy = 0; iy < 9; iy++) {
km[iy + 9 * ix] = kk[iy + 9 * ix] + lvm[iy];
}
}
/* 'gaussmixp:63' mx=max(py,[],1); */
ix = -8;
iy = -1;
for (i = 0; i < 167; i++) {
ix += 9;
ixstart = ix;
ixstop = ix + 8;
mtmp = km[ix - 1];
if (rtIsNaN(km[ix - 1])) {
b_ix = ix;
exitg1 = 0U;
while ((exitg1 == 0U) && (b_ix + 1 <= ixstop)) {
ixstart = b_ix + 1;
if (!rtIsNaN(km[b_ix])) {
mtmp = km[b_ix];
exitg1 = 1U;
} else {
b_ix++;
}
}
}
if (ixstart < ixstop) {
while (ixstart + 1 <= ixstop) {
if (km[ixstart] > mtmp) {
mtmp = km[ixstart];
}
ixstart++;
}
}
iy++;
mx[iy] = mtmp;
}
/* find normalizing factor for each data point to prevent underflow when using exp() */
/* 'gaussmixp:64' px=exp(py-mx(wk,:)); */
for (ix = 0; ix < 167; ix++) {
for (iy = 0; iy < 9; iy++) {
kk[iy + 9 * ix] = km[iy + 9 * ix] - mx[ix];
}
}
b_exp(kk);
/* find normalized probability of each mixture for each datapoint */
/* 'gaussmixp:65' ps=sum(px,1); */
c_sum(kk, ps);
/* total normalized likelihood of each data point */
/* 'gaussmixp:66' lp(ii)=log(ps)+mx; */
d_log(ps);
for (ix = 0; ix < 167; ix++) {
/* 'gaussmixp:67' lp=lp-0.5*q*log(2*pi); */
lp[ix] = (ps[ix] + mx[ix]) - 11.027262398456072;
}
}
/* Function Definitions */
static void b_euclid(sammonStackData *SD, const real_T x[2000], const real_T y
[2000], real_T d[1000000])
{
real_T b_y[2000];
real_T c_y[2000];
int32_T k;
real_T b[2000];
int32_T i2;
real_T alpha1;
real_T beta1;
char_T TRANSB;
char_T TRANSA;
ptrdiff_t m_t;
ptrdiff_t n_t;
ptrdiff_t k_t;
ptrdiff_t lda_t;
ptrdiff_t ldb_t;
ptrdiff_t ldc_t;
double * alpha1_t;
double * Aia0_t;
double * Bib0_t;
double * beta1_t;
double * Cic0_t;
real_T dv4[1000];
real_T dv5[1000];
/* all done */
emlrtPushRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal);
for (k = 0; k < 2000; k++) {
b_y[k] = x[k] * x[k];
c_y[k] = y[k] * y[k];
}
for (k = 0; k < 1000; k++) {
for (i2 = 0; i2 < 2; i2++) {
b[i2 + (k << 1)] = y[k + 1000 * i2];
}
}
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
memset(&SD->u1.f0.y[0], 0, 1000000U * sizeof(real_T));
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&x[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&b[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&SD->u1.f0.y[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
c_sum(b_y, dv4);
c_sum(c_y, dv5);
for (k = 0; k < 1000; k++) {
for (i2 = 0; i2 < 1000; i2++) {
SD->u1.f0.dv6[k + 1000 * i2] = dv4[k];
SD->u1.f0.dv7[k + 1000 * i2] = dv5[i2];
}
}
for (k = 0; k < 1000; k++) {
for (i2 = 0; i2 < 1000; i2++) {
d[i2 + 1000 * k] = (SD->u1.f0.dv6[i2 + 1000 * k] + SD->u1.f0.dv7[i2 + 1000
* k]) - 2.0 * SD->u1.f0.y[i2 + 1000 * k];
}
}
b_sqrt(d);
emlrtPopRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal);
}
void sammon(const emxArray_real_T *x, emxArray_real_T *y)
{
emxArray_real_T *D;
emxArray_real_T *delta;
int32_T i0;
int32_T b_D;
int32_T br;
emxArray_real_T *Dinv;
emxArray_real_T *d;
emxArray_real_T *dinv;
emxArray_real_T *c_D;
emxArray_real_T *b_delta;
emxArray_real_T *b_x;
real_T E;
int32_T i;
emxArray_real_T *deltaone;
emxArray_real_T *g;
emxArray_real_T *y2;
emxArray_real_T *H;
emxArray_real_T *s;
emxArray_real_T *C;
emxArray_real_T *b_C;
emxArray_real_T *c_delta;
emxArray_real_T *d_D;
emxArray_real_T *b_deltaone;
boolean_T exitg1;
boolean_T guard2 = FALSE;
uint32_T unnamed_idx_0;
int32_T ic;
int32_T ar;
int32_T ib;
int32_T ia;
int32_T i1;
boolean_T guard1 = FALSE;
int32_T exitg3;
boolean_T exitg2;
int32_T b_y[2];
real_T E_new;
real_T u1;
emxInit_real_T(&D, 2);
emxInit_real_T(&delta, 2);
/* #codgen */
/* */
/* SAMMON - apply Sammon's nonlinear mapping */
/* */
/* Y = SAMMON(X) applies Sammon's nonlinear mapping procedure on */
/* multivariate data X, where each row represents a pattern and each column */
/* represents a feature. On completion, Y contains the corresponding */
/* co-ordinates of each point on the map. By default, a two-dimensional */
/* map is created. Note if X contains any duplicated rows, SAMMON will */
/* fail (ungracefully). */
/* */
/* [Y,E] = SAMMON(X) also returns the value of the cost function in E (i.e. */
/* the stress of the mapping). */
/* */
/* An N-dimensional output map is generated by Y = SAMMON(X,N) . */
/* */
/* A set of optimisation options can also be specified using a third */
/* argument, Y = SAMMON(X,N,OPTS) , where OPTS is a structure with fields: */
/* */
/* MaxIter - maximum number of iterations */
/* TolFun - relative tolerance on objective function */
/* MaxHalves - maximum number of step halvings */
/* Input - {'raw','distance'} if set to 'distance', X is */
/* interpreted as a matrix of pairwise distances. */
/* Display - {'off', 'on', 'iter'} */
/* Initialisation - {'pca', 'random'} */
/* */
/* The default options structure can be retrieved by calling SAMMON with */
/* no parameters. */
/* */
/* References : */
/* */
/* [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data Structure */
/* Analysis", IEEE Transactions on Computers, vol. C-18, no. 5, */
/* pp 401-409, May 1969. */
/* */
/* See also : SAMMON_TEST */
/* */
/* File : sammon.m */
/* */
/* Date : Monday 12th November 2007. */
/* */
/* Author : Gavin C. Cawley and Nicola L. C. Talbot */
/* */
/* Description : Simple vectorised MATLAB implementation of Sammon's non-linear */
/* mapping algorithm [1]. */
/* */
/* References : [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data */
/* Structure Analysis", IEEE Transactions on Computers, */
/* vol. C-18, no. 5, pp 401-409, May 1969. */
/* */
/* History : 10/08/2004 - v1.00 */
/* 11/08/2004 - v1.10 Hessian made positive semidefinite */
/* 13/08/2004 - v1.11 minor optimisation */
/* 12/11/2007 - v1.20 initialisation using the first n principal */
/* components. */
/* */
/* Thanks : Dr Nick Hamilton ([email protected]) for supplying the */
/* code for implementing initialisation using the first n */
/* principal components (introduced in v1.20). */
/* */
/* To do : The current version does not take advantage of the symmetry */
/* of the distance matrix in order to allow for easy */
/* vectorisation. This may not be a good choice for very large */
/* datasets, so perhaps one day I'll get around to doing a MEX */
/* version using the BLAS library etc. for very large datasets. */
/* */
/* Copyright : (c) Dr Gavin C. Cawley, November 2007. */
/* */
/* This program is free software; you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; either version 2 of the License, or */
/* (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with this program; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */
/* */
/* use the default options structure */
/* create distance matrix unless given by parameters */
euclid(x, x, D);
/* remaining initialisation */
eye(x->size[0], delta);
i0 = D->size[0] * D->size[1];
emxEnsureCapacity((emxArray__common *)D, i0, (int32_T)sizeof(real_T));
b_D = D->size[0];
br = D->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
D->data[i0] += delta->data[i0];
}
emxInit_real_T(&Dinv, 2);
emxInit_real_T(&d, 2);
rdivide(D, Dinv);
randn(x->size[0], y);
b_euclid(y, y, d);
eye(x->size[0], delta);
i0 = d->size[0] * d->size[1];
emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T));
b_D = d->size[0];
br = d->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
d->data[i0] += delta->data[i0];
}
emxInit_real_T(&dinv, 2);
emxInit_real_T(&c_D, 2);
rdivide(d, dinv);
i0 = c_D->size[0] * c_D->size[1];
c_D->size[0] = D->size[0];
c_D->size[1] = D->size[1];
emxEnsureCapacity((emxArray__common *)c_D, i0, (int32_T)sizeof(real_T));
br = D->size[0] * D->size[1];
for (i0 = 0; i0 < br; i0++) {
c_D->data[i0] = D->data[i0] - d->data[i0];
}
emxInit_real_T(&b_delta, 2);
power(c_D, delta);
i0 = b_delta->size[0] * b_delta->size[1];
b_delta->size[0] = delta->size[0];
b_delta->size[1] = delta->size[1];
emxEnsureCapacity((emxArray__common *)b_delta, i0, (int32_T)sizeof(real_T));
br = delta->size[0] * delta->size[1];
emxFree_real_T(&c_D);
for (i0 = 0; i0 < br; i0++) {
b_delta->data[i0] = delta->data[i0] * Dinv->data[i0];
}
emxInit_real_T(&b_x, 2);
c_sum(b_delta, b_x);
E = d_sum(b_x);
/* get on with it */
i = 0;
emxFree_real_T(&b_delta);
emxInit_real_T(&deltaone, 2);
emxInit_real_T(&g, 2);
emxInit_real_T(&y2, 2);
emxInit_real_T(&H, 2);
b_emxInit_real_T(&s, 1);
emxInit_real_T(&C, 2);
emxInit_real_T(&b_C, 2);
emxInit_real_T(&c_delta, 2);
emxInit_real_T(&d_D, 2);
b_emxInit_real_T(&b_deltaone, 1);
exitg1 = FALSE;
while ((exitg1 == FALSE) && (i < 500)) {
/* compute gradient, Hessian and search direction (note it is actually */
/* 1/4 of the gradient and Hessian, but the step size is just the ratio */
/* of the gradient and the diagonal of the Hessian so it doesn't matter). */
i0 = delta->size[0] * delta->size[1];
delta->size[0] = dinv->size[0];
delta->size[1] = dinv->size[1];
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
br = dinv->size[0] * dinv->size[1];
for (i0 = 0; i0 < br; i0++) {
delta->data[i0] = dinv->data[i0] - Dinv->data[i0];
}
guard2 = FALSE;
if (delta->size[1] == 1) {
guard2 = TRUE;
} else {
b_D = x->size[0];
if (b_D == 1) {
guard2 = TRUE;
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = (int32_T)unnamed_idx_0;
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof
(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
deltaone->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
deltaone->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br + 1; ib <= i0; ib++) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
deltaone->data[ic] += delta->data[ia - 1];
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
}
if (guard2 == TRUE) {
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = delta->size[0];
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
deltaone->data[i0 + deltaone->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
deltaone->data[i0 + deltaone->size[0] * i1] += delta->data[i0 +
delta->size[0] * b_D];
}
}
}
}
if ((delta->size[1] == 1) || (y->size[0] == 1)) {
i0 = g->size[0] * g->size[1];
g->size[0] = delta->size[0];
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
g->data[i0 + g->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
g->data[i0 + g->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y->data[b_D + y->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = g->size[0] * g->size[1];
g->size[0] = (int32_T)unnamed_idx_0;
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
g->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
g->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
g->data[ic] += y->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
i0 = g->size[0] * g->size[1];
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
b_D = g->size[0];
br = g->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
g->data[i0] -= y->data[i0] * deltaone->data[i0];
}
c_power(dinv, delta);
b_power(y, y2);
if ((delta->size[1] == 1) || (y2->size[0] == 1)) {
i0 = H->size[0] * H->size[1];
H->size[0] = delta->size[0];
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
H->data[i0 + H->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
H->data[i0 + H->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y2->data[b_D + y2->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = H->size[0] * H->size[1];
H->size[0] = (int32_T)unnamed_idx_0;
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
H->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
H->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y2->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
H->data[ic] += y2->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
if ((delta->size[1] == 1) || (y->size[0] == 1)) {
i0 = C->size[0] * C->size[1];
C->size[0] = delta->size[0];
C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
C->data[i0 + C->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
C->data[i0 + C->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y->data[b_D + y->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = C->size[0] * C->size[1];
C->size[0] = (int32_T)unnamed_idx_0;
C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
C->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
C->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
C->data[ic] += y->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
guard1 = FALSE;
if (delta->size[1] == 1) {
guard1 = TRUE;
} else {
b_D = x->size[0];
if (b_D == 1) {
guard1 = TRUE;
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = b_C->size[0] * b_C->size[1];
b_C->size[0] = (int32_T)unnamed_idx_0;
b_C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
b_C->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
b_C->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br + 1; ib <= i0; ib++) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
b_C->data[ic] += delta->data[ia - 1];
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
}
if (guard1 == TRUE) {
i0 = b_C->size[0] * b_C->size[1];
b_C->size[0] = delta->size[0];
b_C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
b_C->data[i0 + b_C->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
b_C->data[i0 + b_C->size[0] * i1] += delta->data[i0 + delta->size[0]
* b_D];
}
}
}
}
i0 = H->size[0] * H->size[1];
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
b_D = H->size[0];
br = H->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
H->data[i0] = ((H->data[i0] - deltaone->data[i0]) - 2.0 * y->data[i0] *
C->data[i0]) + y2->data[i0] * b_C->data[i0];
}
b_D = H->size[0] << 1;
i0 = s->size[0];
s->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = 0;
do {
exitg3 = 0;
b_D = H->size[0] << 1;
if (br <= b_D - 1) {
s->data[br] = fabs(H->data[br]);
br++;
} else {
exitg3 = 1;
}
} while (exitg3 == 0);
i0 = s->size[0];
s->size[0] = g->size[0] << 1;
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = g->size[0] << 1;
for (i0 = 0; i0 < br; i0++) {
s->data[i0] = -g->data[i0] / s->data[i0];
}
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = y->size[0];
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T));
br = y->size[0] * y->size[1];
for (i0 = 0; i0 < br; i0++) {
deltaone->data[i0] = y->data[i0];
}
/* use step-halving procedure to ensure progress is made */
ib = 1;
ia = 0;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (ia < 20)) {
ib = ia + 1;
b_D = deltaone->size[0] << 1;
i0 = b_deltaone->size[0];
b_deltaone->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)b_deltaone, i0, (int32_T)sizeof
(real_T));
for (i0 = 0; i0 < b_D; i0++) {
b_deltaone->data[i0] = deltaone->data[i0] + s->data[i0];
}
for (i0 = 0; i0 < 2; i0++) {
b_y[i0] = y->size[i0];
}
i0 = y->size[0] * y->size[1];
y->size[0] = b_y[0];
y->size[1] = b_y[1];
emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T));
br = b_y[1];
for (i0 = 0; i0 < br; i0++) {
ar = b_y[0];
for (i1 = 0; i1 < ar; i1++) {
y->data[i1 + y->size[0] * i0] = b_deltaone->data[i1 + b_y[0] * i0];
}
}
b_euclid(y, y, d);
b_D = x->size[0];
i0 = delta->size[0] * delta->size[1];
delta->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
b_D = x->size[0];
i0 = delta->size[0] * delta->size[1];
delta->size[1] = b_D;
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
br = x->size[0] * x->size[0];
for (i0 = 0; i0 < br; i0++) {
delta->data[i0] = 0.0;
}
E_new = x->size[0];
u1 = x->size[0];
if (E_new <= u1) {
} else {
E_new = u1;
}
b_D = (int32_T)E_new;
if (b_D > 0) {
for (br = 0; br + 1 <= b_D; br++) {
delta->data[br + delta->size[0] * br] = 1.0;
}
}
i0 = d->size[0] * d->size[1];
emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T));
b_D = d->size[0];
br = d->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
d->data[i0] += delta->data[i0];
}
rdivide(d, dinv);
i0 = d_D->size[0] * d_D->size[1];
d_D->size[0] = D->size[0];
d_D->size[1] = D->size[1];
emxEnsureCapacity((emxArray__common *)d_D, i0, (int32_T)sizeof(real_T));
br = D->size[0] * D->size[1];
for (i0 = 0; i0 < br; i0++) {
d_D->data[i0] = D->data[i0] - d->data[i0];
}
power(d_D, delta);
i0 = c_delta->size[0] * c_delta->size[1];
c_delta->size[0] = delta->size[0];
c_delta->size[1] = delta->size[1];
emxEnsureCapacity((emxArray__common *)c_delta, i0, (int32_T)sizeof(real_T));
br = delta->size[0] * delta->size[1];
for (i0 = 0; i0 < br; i0++) {
c_delta->data[i0] = delta->data[i0] * Dinv->data[i0];
}
c_sum(c_delta, b_x);
if (b_x->size[1] == 0) {
E_new = 0.0;
} else {
E_new = b_x->data[0];
for (br = 2; br <= b_x->size[1]; br++) {
E_new += b_x->data[br - 1];
}
}
if (E_new < E) {
exitg2 = TRUE;
} else {
i0 = s->size[0];
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = s->size[0];
for (i0 = 0; i0 < br; i0++) {
s->data[i0] *= 0.5;
}
ia++;
}
}
/* bomb out if too many halving steps are required */
if ((ib == 20) || (fabs((E - E_new) / E) < 1.0E-9)) {
exitg1 = TRUE;
} else {
/* evaluate termination criterion */
/* report progress */
E = E_new;
i++;
}
}
emxFree_real_T(&b_deltaone);
emxFree_real_T(&d_D);
emxFree_real_T(&c_delta);
emxFree_real_T(&b_x);
emxFree_real_T(&b_C);
emxFree_real_T(&C);
emxFree_real_T(&s);
emxFree_real_T(&H);
emxFree_real_T(&y2);
emxFree_real_T(&g);
emxFree_real_T(&deltaone);
emxFree_real_T(&delta);
emxFree_real_T(&dinv);
emxFree_real_T(&d);
emxFree_real_T(&Dinv);
emxFree_real_T(&D);
/* fiddle stress to match the original Sammon paper */
}
